diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp index 5661ae6d..27a55021 100644 --- a/Grbl_Esp32/Custom/atari_1020.cpp +++ b/Grbl_Esp32/Custom/atari_1020.cpp @@ -57,7 +57,8 @@ void machine_init() { NULL, // parameters 1, // priority &solenoidSyncTaskHandle, - 0 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); // setup a task that will do the custom homing sequence xTaskCreatePinnedToCore(atari_home_task, // task @@ -66,7 +67,8 @@ void machine_init() { NULL, // parameters 1, // priority &atariHomingTaskHandle, - 0 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); } diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 6ca3728c..3421c670 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -159,6 +159,10 @@ # define DEFAULT_LASER_MODE 0 // false #endif +#ifndef DEFAULT_LASER_FULL_POWER +# define DEFAULT_LASER_FULL_POWER 1000 +#endif + #ifndef DEFAULT_SPINDLE_RPM_MAX // $30 # define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm #endif diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 647e0c06..e5b624da 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -23,7 +23,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20201124"; +const char* const GRBL_VERSION_BUILD = "20201128"; //#include #include diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index b86c0c26..e3a8e1c6 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -112,13 +112,22 @@ Socket #5 // #define Y_LIMIT_PIN GPIO_NUM_25 // #define Z_LIMIT_PIN GPIO_NUM_39 -// // 4x Input Module in Socket #3 -// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -// #define CONTROL_CYCLE_START_PIN GPIO_NUM_26 -// #define CONTROL_FEED_HOLD_PIN GPIO_NUM_4 -// #define CONTROL_RESET_PIN GPIO_NUM_16 -// #define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_27 -// //#define INVERT_CONTROL_PIN_MASK B0000 +// 5V output CNC module in socket #4 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module +#define SPINDLE_TYPE SpindleType::PWM +#define SPINDLE_OUTPUT_PIN GPIO_NUM_14 +#define SPINDLE_ENABLE_PIN GPIO_NUM_13 // optional +#define LASER_OUTPUT_PIN GPIO_NUM_15 // optional +#define LASER_ENABLE_PIN GPIO_NUM_12 + + + + +// // RS485 Modbus In socket #3 +// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/RS485-Modbus-Module +// #define VFD_RS485_TXD_PIN GPIO_NUM_26 +// #define VFD_RS485_RTS_PIN GPIO_NUM_4 +// #define VFD_RS485_RXD_PIN GPIO_NUM_16 // ================= Setting Defaults ========================== #define DEFAULT_X_STEPS_PER_MM 800 diff --git a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h index f7b02d29..43f2864c 100644 --- a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h +++ b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h @@ -53,8 +53,8 @@ // OK to comment out to use pin for other features #define STEPPERS_DISABLE_PIN GPIO_NUM_13 -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_16 +#define SPINDLE_TYPE SpindleType::LASER +#define LASER_OUTPUT_PIN GPIO_NUM_16 #define COOLANT_MIST_PIN GPIO_NUM_2 diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index 8c7d550a..1a08c0c8 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -43,16 +43,16 @@ #include "Dynamixel2.h" #include "TrinamicDriver.h" -Motors::Motor* myMotor[MAX_AXES][MAX_GANGED]; // number of axes (normal and ganged) -void init_motors() { +Motors::Motor* myMotor[MAX_AXES][MAX_GANGED]; // number of axes (normal and ganged) +void init_motors() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Init Motors"); auto n_axis = number_axis->get(); if (n_axis >= 1) { #ifdef X_TRINAMIC_DRIVER - myMotor[X_AXIS][0] = new Motors::TrinamicDriver( - X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE); + myMotor[X_AXIS][0] = + new Motors::TrinamicDriver(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE); #elif defined(X_SERVO_PIN) myMotor[X_AXIS][0] = new Motors::RcServo(X_AXIS, X_SERVO_PIN); #elif defined(X_UNIPOLAR) @@ -66,8 +66,8 @@ void init_motors() { #endif #ifdef X2_TRINAMIC_DRIVER - myMotor[X_AXIS][1] = new Motors::TrinamicDriver( - X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE); + myMotor[X_AXIS][1] = + new Motors::TrinamicDriver(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE); #elif defined(X2_UNIPOLAR) myMotor[X_AXIS][1] = new Motors::UnipolarMotor(X2_AXIS, X2_PIN_PHASE_0, X2_PIN_PHASE_1, X2_PIN_PHASE_2, X2_PIN_PHASE_3); #elif defined(X2_STEP_PIN) @@ -83,8 +83,8 @@ void init_motors() { if (n_axis >= 2) { // this WILL be done better with settings #ifdef Y_TRINAMIC_DRIVER - myMotor[Y_AXIS][0] = new Motors::TrinamicDriver( - Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE); + myMotor[Y_AXIS][0] = + new Motors::TrinamicDriver(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE); #elif defined(Y_SERVO_PIN) myMotor[Y_AXIS][0] = new Motors::RcServo(Y_AXIS, Y_SERVO_PIN); #elif defined(Y_UNIPOLAR) @@ -98,8 +98,8 @@ void init_motors() { #endif #ifdef Y2_TRINAMIC_DRIVER - myMotor[Y_AXIS][1] = new Motors::TrinamicDriver( - Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE); + myMotor[Y_AXIS][1] = + new Motors::TrinamicDriver(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE); #elif defined(Y2_UNIPOLAR) myMotor[Y_AXIS][1] = new Motors::UnipolarMotor(Y2_AXIS, Y2_PIN_PHASE_0, Y2_PIN_PHASE_1, Y2_PIN_PHASE_2, Y2_PIN_PHASE_3); #elif defined(Y2_STEP_PIN) @@ -115,8 +115,8 @@ void init_motors() { if (n_axis >= 3) { // this WILL be done better with settings #ifdef Z_TRINAMIC_DRIVER - myMotor[Z_AXIS][0] = new Motors::TrinamicDriver( - Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE); + myMotor[Z_AXIS][0] = + new Motors::TrinamicDriver(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE); #elif defined(Z_SERVO_PIN) myMotor[Z_AXIS][0] = new Motors::RcServo(Z_AXIS, Z_SERVO_PIN); #elif defined(Z_UNIPOLAR) @@ -130,8 +130,8 @@ void init_motors() { #endif #ifdef Z2_TRINAMIC_DRIVER - myMotor[Z_AXIS][1] = new Motors::TrinamicDriver( - Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE); + myMotor[Z_AXIS][1] = + new Motors::TrinamicDriver(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE); #elif defined(Z2_UNIPOLAR) myMotor[Z_AXIS][1] = new Motors::UnipolarMotor(Z2_AXIS, Z2_PIN_PHASE_0, Z2_PIN_PHASE_1, Z2_PIN_PHASE_2, Z2_PIN_PHASE_3); #elif defined(Z2_STEP_PIN) @@ -147,8 +147,8 @@ void init_motors() { if (n_axis >= 4) { // this WILL be done better with settings #ifdef A_TRINAMIC_DRIVER - myMotor[A_AXIS][0] = new Motors::TrinamicDriver( - A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE); + myMotor[A_AXIS][0] = + new Motors::TrinamicDriver(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE); #elif defined(A_SERVO_PIN) myMotor[A_AXIS][0] = new Motors::RcServo(A_AXIS, A_SERVO_PIN); #elif defined(A_UNIPOLAR) @@ -162,8 +162,8 @@ void init_motors() { #endif #ifdef A2_TRINAMIC_DRIVER - myMotor[A_AXIS][1] = new Motors::TrinamicDriver( - A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE); + myMotor[A_AXIS][1] = + new Motors::TrinamicDriver(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE); #elif defined(A2_UNIPOLAR) myMotor[A_AXIS][1] = new Motors::UnipolarMotor(A2_AXIS, A2_PIN_PHASE_0, A2_PIN_PHASE_1, A2_PIN_PHASE_2, A2_PIN_PHASE_3); #elif defined(A2_STEP_PIN) @@ -179,8 +179,8 @@ void init_motors() { if (n_axis >= 5) { // this WILL be done better with settings #ifdef B_TRINAMIC_DRIVER - myMotor[B_AXIS][0] = new Motors::TrinamicDriver( - B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE); + myMotor[B_AXIS][0] = + new Motors::TrinamicDriver(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE); #elif defined(B_SERVO_PIN) myMotor[B_AXIS][0] = new Motors::RcServo(B_AXIS, B_SERVO_PIN); #elif defined(B_UNIPOLAR) @@ -194,8 +194,8 @@ void init_motors() { #endif #ifdef B2_TRINAMIC_DRIVER - myMotor[B_AXIS][1] = new Motors::TrinamicDriver( - B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE); + myMotor[B_AXIS][1] = + new Motors::TrinamicDriver(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE); #elif defined(B2_UNIPOLAR) myMotor[B_AXIS][1] = new Motors::UnipolarMotor(B2_AXIS, B2_PIN_PHASE_0, B2_PIN_PHASE_1, B2_PIN_PHASE_2, B2_PIN_PHASE_3); #elif defined(B2_STEP_PIN) @@ -211,8 +211,8 @@ void init_motors() { if (n_axis >= 6) { // this WILL be done better with settings #ifdef C_TRINAMIC_DRIVER - myMotor[C_AXIS][0] = new Motors::TrinamicDriver( - C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE); + myMotor[C_AXIS][0] = + new Motors::TrinamicDriver(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE); #elif defined(C_SERVO_PIN) myMotor[C_AXIS][0] = new Motors::RcServo(C_AXIS, C_SERVO_PIN); #elif defined(C_UNIPOLAR) @@ -226,8 +226,8 @@ void init_motors() { #endif #ifdef C2_TRINAMIC_DRIVER - myMotor[C_AXIS][1] = new Motors::TrinamicDriver( - C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE); + myMotor[C_AXIS][1] = + new Motors::TrinamicDriver(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE); #elif defined(C2_UNIPOLAR) myMotor[C_AXIS][1] = new Motors::UnipolarMotor(C2_AXIS, C2_PIN_PHASE_0, C2_PIN_PHASE_1, C2_PIN_PHASE_2, C2_PIN_PHASE_3); #elif defined(C2_STEP_PIN) @@ -279,23 +279,16 @@ void init_motors() { } } -void motors_set_disable(bool disable) { +void motors_set_disable(bool disable, uint8_t mask) { static bool previous_state = true; - //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Motors disable %d", disable); - - /* - if (previous_state == disable) { - return; - } - previous_state = disable; -*/ - // now loop through all the motors to see if they can individually disable auto n_axis = number_axis->get(); for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) { for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - myMotor[axis][gang_index]->set_disable(disable); + if (bitnum_istrue(mask, axis)) { + myMotor[axis][gang_index]->set_disable(disable); + } } } @@ -320,7 +313,7 @@ void motors_read_settings() { // They can use this to setup things like Stall uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming) { uint8_t can_home = 0; - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { if (bitnum_istrue(homing_mask, axis)) { if (myMotor[axis][0]->set_homing_mode(isHoming)) { diff --git a/Grbl_Esp32/src/Motors/Motors.h b/Grbl_Esp32/src/Motors/Motors.h index a6451eff..14550f68 100644 --- a/Grbl_Esp32/src/Motors/Motors.h +++ b/Grbl_Esp32/src/Motors/Motors.h @@ -39,7 +39,7 @@ void motors_read_settings(); // The return value is a bitmask of axes that can home uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming); -void motors_set_disable(bool disable); +void motors_set_disable(bool disable, uint8_t mask = B11111111); // default is all axes void motors_step(uint8_t step_mask, uint8_t dir_mask); void motors_unstep(); diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index af0f39e7..9c808c4e 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -115,7 +115,8 @@ namespace Motors { NULL, // parameters 1, // priority NULL, - 0 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); } } diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index f8539203..2fe07b5a 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -389,6 +389,38 @@ Error listErrors(const char* value, WebUI::AuthenticationLevel auth_level, WebUI return Error::Ok; } +Error motor_disable(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { + char* s; + if (value == NULL) { + value = "\0"; + } + + s = strdup(value); + s = trim(s); + + int32_t convertedValue; + char* endptr; + if (*s == '\0') { + convertedValue = 255; // all axes + } else { + convertedValue = strtol(s, &endptr, 10); + if (endptr == s || *endptr != '\0') { + // Try to convert as an axis list + convertedValue = 0; + auto axisNames = String("XYZABC"); + while (*s) { + int index = axisNames.indexOf(toupper(*s++)); + if (index < 0) { + return Error::BadNumberFormat; + } + convertedValue |= bit(index); + } + } + } + motors_set_disable(true, convertedValue); + return Error::Ok; +} + static bool anyState() { return false; } @@ -427,6 +459,8 @@ void make_grbl_commands() { new GrblCommand("V", "Settings/Stats", Setting::report_nvs_stats, idleOrAlarm); new GrblCommand("#", "GCode/Offsets", report_ngc, idleOrAlarm); new GrblCommand("H", "Home", home_all, idleOrAlarm); + new GrblCommand("MD", "Motor/Disable", motor_disable, idleOrAlarm); + #ifdef HOMING_SINGLE_AXIS_COMMANDS new GrblCommand("HX", "Home/X", home_x, idleOrAlarm); new GrblCommand("HY", "Home/Y", home_y, idleOrAlarm); diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index caa78bb6..8a1403cf 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -104,7 +104,8 @@ void serial_init() { NULL, // parameters 1, // priority &serialCheckTaskHandle, - 1 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); } diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index 5450b444..ce7af34a 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -126,6 +126,7 @@ Error IntSetting::setStringValue(char* s) { _storedValue = convertedValue; } } + check(NULL); return Error::Ok; } @@ -223,6 +224,7 @@ Error AxisMaskSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -323,6 +325,7 @@ Error FloatSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -412,6 +415,7 @@ Error StringSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -442,11 +446,15 @@ void StringSetting::addWebui(WebUI::JSONencoder* j) { typedef std::map enum_opt_t; -EnumSetting::EnumSetting( - const char* description, type_t type, permissions_t permissions, const char* grblName, const char* name, int8_t defVal, enum_opt_t* opts) - // No checker function because enumerations have an exact set of value - : - Setting(description, type, permissions, grblName, name, NULL), +EnumSetting::EnumSetting(const char* description, + type_t type, + permissions_t permissions, + const char* grblName, + const char* name, + int8_t defVal, + enum_opt_t* opts, + bool (*checker)(char*) = NULL) : + Setting(description, type, permissions, grblName, name, checker), _defaultValue(defVal), _options(opts) {} void EnumSetting::load() { @@ -471,7 +479,11 @@ void EnumSetting::setDefault() { // This is necessary for WebUI, which uses the number // for setting. Error EnumSetting::setStringValue(char* s) { - s = trim(s); + s = trim(s); + Error err = check(s); + if (err != Error::Ok) { + return err; + } enum_opt_t::iterator it = _options->find(s); if (it == _options->end()) { // If we don't find the value in keys, look for it in the numeric values @@ -497,7 +509,7 @@ Error EnumSetting::setStringValue(char* s) { } _currentValue = it->second; if (_storedValue != _currentValue) { - if (_storedValue == _defaultValue) { + if (_currentValue == _defaultValue) { nvs_erase_key(_handle, _keyName); } else { if (nvs_set_i8(_handle, _keyName, _currentValue)) { @@ -506,6 +518,7 @@ Error EnumSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -585,6 +598,7 @@ Error FlagSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } const char* FlagSetting::getDefaultString() { @@ -665,6 +679,7 @@ Error IPaddrSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index 63dce4f5..8e02a125 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -343,10 +343,17 @@ public: const char* grblName, const char* name, int8_t defVal, - enum_opt_t* opts); + enum_opt_t* opts, + bool (*checker)(char*)); - EnumSetting(type_t type, permissions_t permissions, const char* grblName, const char* name, int8_t defVal, enum_opt_t* opts) : - EnumSetting(NULL, type, permissions, grblName, name, defVal, opts) {} + EnumSetting(type_t type, + permissions_t permissions, + const char* grblName, + const char* name, + int8_t defVal, + enum_opt_t* opts, + bool (*checker)(char*) = NULL) : + EnumSetting(NULL, type, permissions, grblName, name, defVal, opts, checker) {} void load(); void setDefault(); diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 5e2c2e2c..0a11cdee 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -32,6 +32,7 @@ FlagSetting* homing_enable; // TODO Settings - also need to clear, but not set, soft_limits FlagSetting* laser_mode; // TODO Settings - also need to call my_spindle->init; +IntSetting* laser_full_power; IntSetting* status_mask; FloatSetting* junction_deviation; @@ -166,34 +167,32 @@ static const char* makename(const char* axisName, const char* tail) { } static bool checkStartupLine(char* value) { + if (!value) { // No POST functionality + return true; + } if (sys.state != State::Idle) { return false; } return gc_execute_line(value, CLIENT_SERIAL) == Error::Ok; } -static bool checkStallguard(char* value) { - motorSettingChanged = true; +static bool postTMC(char* value) { + if (!value) { // No POST functionality + motorSettingChanged = true; + } return true; } -static bool checkMicrosteps(char* value) { - motorSettingChanged = true; - return true; -} - -static bool checkRunCurrent(char* value) { - motorSettingChanged = true; - return true; -} - -static bool checkHoldcurrent(char* value) { - motorSettingChanged = true; - return true; -} - -static bool checkStallguardDebugMask(char* val) { - motorSettingChanged = true; +static bool checkSpindleChange(char* val) { + if (!val) { + spindle->deinit(); + Spindles::Spindle::select(); + return true; + } + if (gc_state.modal.spindle != SpindleState::Disable) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle must be off to make this change"); + return false; + } return true; } @@ -209,8 +208,8 @@ static const char* makeGrblName(int axisNum, int base) { void make_coordinate(CoordIndex index, const char* name) { float coord_data[MAX_N_AXIS] = { 0.0 }; - auto coord = new Coordinates(name); - coords[index] = coord; + auto coord = new Coordinates(name); + coords[index] = coord; if (!coord->load()) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Propagating %s data to NVS format", coord->getName()); // If coord->load() returns false it means that no entry @@ -259,30 +258,30 @@ void make_settings() { b_axis_settings = axis_settings[B_AXIS]; c_axis_settings = axis_settings[C_AXIS]; for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { - def = &axis_defaults[axis]; - auto setting = new IntSetting( - EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 63, checkStallguard); + def = &axis_defaults[axis]; + auto setting = + new IntSetting(EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 63, postTMC); setting->setAxis(axis); axis_settings[axis]->stallguard = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { - def = &axis_defaults[axis]; - auto setting = new IntSetting( - EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, checkMicrosteps); + def = &axis_defaults[axis]; + auto setting = + new IntSetting(EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, postTMC); setting->setAxis(axis); axis_settings[axis]->microsteps = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new FloatSetting( - EXTENDED, WG, makeGrblName(axis, 150), makename(def->name, "Current/Hold"), def->hold_current, 0.05, 20.0, checkHoldcurrent); // Amps + EXTENDED, WG, makeGrblName(axis, 150), makename(def->name, "Current/Hold"), def->hold_current, 0.05, 20.0, postTMC); // Amps setting->setAxis(axis); axis_settings[axis]->hold_current = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new FloatSetting( - EXTENDED, WG, makeGrblName(axis, 140), makename(def->name, "Current/Run"), def->run_current, 0.0, 20.0, checkRunCurrent); // Amps + EXTENDED, WG, makeGrblName(axis, 140), makename(def->name, "Current/Run"), def->run_current, 0.0, 20.0, postTMC); // Amps setting->setAxis(axis); axis_settings[axis]->run_current = setting; } @@ -322,31 +321,38 @@ void make_settings() { } // Spindle Settings - spindle_pwm_max_value = new FloatSetting(EXTENDED, WG, "36", "Spindle/PWM/Max", DEFAULT_SPINDLE_MAX_VALUE, 0.0, 100.0); - spindle_pwm_min_value = new FloatSetting(EXTENDED, WG, "35", "Spindle/PWM/Min", DEFAULT_SPINDLE_MIN_VALUE, 0.0, 100.0); - spindle_pwm_off_value = - new FloatSetting(EXTENDED, WG, "34", "Spindle/PWM/Off", DEFAULT_SPINDLE_OFF_VALUE, 0.0, 100.0); // these are percentages + spindle_type = + new EnumSetting(NULL, EXTENDED, WG, NULL, "Spindle/Type", static_cast(SPINDLE_TYPE), &spindleTypes, checkSpindleChange); + + spindle_pwm_max_value = + new FloatSetting(EXTENDED, WG, "36", "Spindle/PWM/Max", DEFAULT_SPINDLE_MAX_VALUE, 0.0, 100.0, checkSpindleChange); + spindle_pwm_min_value = + new FloatSetting(EXTENDED, WG, "35", "Spindle/PWM/Min", DEFAULT_SPINDLE_MIN_VALUE, 0.0, 100.0, checkSpindleChange); + spindle_pwm_off_value = new FloatSetting( + EXTENDED, WG, "34", "Spindle/PWM/Off", DEFAULT_SPINDLE_OFF_VALUE, 0.0, 100.0, checkSpindleChange); // these are percentages // IntSetting spindle_pwm_bit_precision(EXTENDED, WG, "Spindle/PWM/Precision", DEFAULT_SPINDLE_BIT_PRECISION, 1, 16); - spindle_pwm_freq = new FloatSetting(EXTENDED, WG, "33", "Spindle/PWM/Frequency", DEFAULT_SPINDLE_FREQ, 0, 100000); - spindle_output_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/PWM/Invert", DEFAULT_INVERT_SPINDLE_OUTPUT_PIN); + spindle_pwm_freq = new FloatSetting(EXTENDED, WG, "33", "Spindle/PWM/Frequency", DEFAULT_SPINDLE_FREQ, 0, 100000, checkSpindleChange); + spindle_output_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/PWM/Invert", DEFAULT_INVERT_SPINDLE_OUTPUT_PIN, checkSpindleChange); spindle_delay_spinup = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30); spindle_delay_spindown = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30); spindle_enbl_off_with_zero_speed = - new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED); + new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED, checkSpindleChange); - spindle_enable_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/Invert", DEFAULT_INVERT_SPINDLE_ENABLE_PIN); + spindle_enable_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/Invert", DEFAULT_INVERT_SPINDLE_ENABLE_PIN, checkSpindleChange); // GRBL Non-numbered settings startup_line_0 = new StringSetting(GRBL, WG, "N0", "GCode/Line0", "", checkStartupLine); startup_line_1 = new StringSetting(GRBL, WG, "N1", "GCode/Line1", "", checkStartupLine); // GRBL Numbered Settings - laser_mode = new FlagSetting(GRBL, WG, "32", "GCode/LaserMode", DEFAULT_LASER_MODE); + laser_mode = new FlagSetting(GRBL, WG, "32", "GCode/LaserMode", DEFAULT_LASER_MODE); + laser_full_power = new IntSetting(EXTENDED, WG, NULL, "Laser/FullPower", DEFAULT_LASER_FULL_POWER, 0, 10000, checkSpindleChange); + // TODO Settings - also need to call my_spindle->init(); - rpm_min = new FloatSetting(GRBL, WG, "31", "GCode/MinS", DEFAULT_SPINDLE_RPM_MIN, 0, 100000); - rpm_max = new FloatSetting(GRBL, WG, "30", "GCode/MaxS", DEFAULT_SPINDLE_RPM_MAX, 0, 100000); + rpm_min = new FloatSetting(GRBL, WG, "31", "GCode/MinS", DEFAULT_SPINDLE_RPM_MIN, 0, 100000, checkSpindleChange); + rpm_max = new FloatSetting(GRBL, WG, "30", "GCode/MaxS", DEFAULT_SPINDLE_RPM_MAX, 0, 100000, checkSpindleChange); homing_pulloff = new FloatSetting(GRBL, WG, "27", "Homing/Pulloff", DEFAULT_HOMING_PULLOFF, 0, 1000); homing_debounce = new FloatSetting(GRBL, WG, "26", "Homing/Debounce", DEFAULT_HOMING_DEBOUNCE_DELAY, 0, 10000); @@ -377,8 +383,8 @@ void make_settings() { step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK); stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255); pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); - spindle_type = new EnumSetting(NULL, EXTENDED, WG, NULL, "Spindle/Type", static_cast(SPINDLE_TYPE), &spindleTypes); - stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, checkStallguardDebugMask); + + stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, postTMC); homing_cycle[0] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle0", DEFAULT_HOMING_CYCLE_0); homing_cycle[1] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle1", DEFAULT_HOMING_CYCLE_1); diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index b01038bf..41599b99 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -35,6 +35,7 @@ extern FlagSetting* soft_limits; extern FlagSetting* hard_limits; extern FlagSetting* homing_enable; extern FlagSetting* laser_mode; +extern IntSetting* laser_full_power; extern IntSetting* status_mask; extern FloatSetting* junction_deviation; diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.cpp b/Grbl_Esp32/src/Spindles/10vSpindle.cpp index c2f050da..dd2284b5 100644 --- a/Grbl_Esp32/src/Spindles/10vSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/10vSpindle.cpp @@ -44,7 +44,7 @@ namespace Spindles { #endif if (_output_pin == UNDEFINED_PIN) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin not defined"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle output pin not defined"); return; // We cannot continue without the output pin } @@ -66,7 +66,7 @@ namespace Spindles { // prints the startup message of the spindle config void _10v::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "0-10V spindle Out:%s Enbl:%s, Dir:%s, Fwd:%s, Rev:%s, Freq:%dHz Res:%dbits", pinName(_output_pin).c_str(), @@ -144,7 +144,6 @@ namespace Spindles { } void _10v::set_enable_pin(bool enable) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle::_10v::set_enable_pin"); if (_off_with_zero_speed && sys.spindle_speed == 0) { enable = false; } @@ -164,9 +163,28 @@ namespace Spindles { } void _10v::set_dir_pin(bool Clockwise) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle::_10v::set_dir_pin"); digitalWrite(_direction_pin, Clockwise); digitalWrite(_forward_pin, Clockwise); digitalWrite(_reverse_pin, !Clockwise); } + + void _10v::deinit() { +#ifdef SPINDLE_OUTPUT_PIN + pinMode(SPINDLE_OUTPUT_PIN, INPUT); +#endif +#ifdef SPINDLE_ENABLE_PIN + pinMode(SPINDLE_ENABLE_PIN, INPUT); +#endif + +#ifdef SPINDLE_DIR_PIN + pinMode(SPINDLE_DIR_PIN, INPUT); +#endif +#ifdef SPINDLE_FORWARD_PIN + pinMode(SPINDLE_FORWARD_PIN, INPUT); +#endif + +#ifdef SPINDLE_REVERSE_PIN + pinMode(SPINDLE_FORWARD_PIN, INPUT); +#endif + } } diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.h b/Grbl_Esp32/src/Spindles/10vSpindle.h index a0205320..d45a9cb5 100644 --- a/Grbl_Esp32/src/Spindles/10vSpindle.h +++ b/Grbl_Esp32/src/Spindles/10vSpindle.h @@ -45,6 +45,7 @@ namespace Spindles { SpindleState get_state() override; void stop() override; + void deinit() override; virtual ~_10v() {} diff --git a/Grbl_Esp32/src/Spindles/BESCSpindle.cpp b/Grbl_Esp32/src/Spindles/BESCSpindle.cpp index 55630bcb..fceb8bc5 100644 --- a/Grbl_Esp32/src/Spindles/BESCSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/BESCSpindle.cpp @@ -55,7 +55,7 @@ namespace Spindles { get_pins_and_settings(); // these gets the standard PWM settings, but many need to be changed for BESC if (_output_pin == UNDEFINED_PIN) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: BESC output pin not defined"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: BESC output pin not defined"); return; // We cannot continue without the output pin } @@ -82,7 +82,7 @@ namespace Spindles { // prints the startup message of the spindle config void BESC::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "BESC spindle on Pin:%s Min:%0.2fms Max:%0.2fms Freq:%dHz Res:%dbits", pinName(_output_pin).c_str(), diff --git a/Grbl_Esp32/src/Spindles/DacSpindle.cpp b/Grbl_Esp32/src/Spindles/DacSpindle.cpp index b679c857..26910ced 100644 --- a/Grbl_Esp32/src/Spindles/DacSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/DacSpindle.cpp @@ -40,7 +40,7 @@ namespace Spindles { if (_output_pin != GPIO_NUM_25 && _output_pin != GPIO_NUM_26) { // DAC can only be used on these pins _gpio_ok = false; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "DAC spindle pin invalid GPIO_NUM_%d (pin 25 or 26 only)", _output_pin); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "DAC spindle pin invalid GPIO_NUM_%d (pin 25 or 26 only)", _output_pin); return; } @@ -54,7 +54,7 @@ namespace Spindles { } void Dac::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "DAC spindle Output:%s, Enbl:%s, Dir:%s, Res:8bits", pinName(_output_pin).c_str(), @@ -85,7 +85,7 @@ namespace Spindles { rpm = _min_rpm; sys.spindle_speed = rpm; pwm_value = 0; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RPM less than min RPM:%5.2f %d", rpm, pwm_value); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RPM less than min RPM:%5.2f %d", rpm, pwm_value); } } else { // Compute intermediate PWM value with linear spindle speed model. diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp index b18249e5..8629965e 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp @@ -98,7 +98,7 @@ namespace Spindles { uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]); vfd->_max_rpm = rpm; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); return true; }; diff --git a/Grbl_Esp32/src/Spindles/Laser.cpp b/Grbl_Esp32/src/Spindles/Laser.cpp index b9fa0c72..befdc0d9 100644 --- a/Grbl_Esp32/src/Spindles/Laser.cpp +++ b/Grbl_Esp32/src/Spindles/Laser.cpp @@ -29,14 +29,71 @@ namespace Spindles { } void Laser::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, - "Laser spindle on Pin:%s, Freq:%dHz, Res:%dbits Laser mode:%s", + "Laser spindle on Pin:%s, Enbl:%s, Freq:%dHz, Res:%dbits Laser mode:%s", pinName(_output_pin).c_str(), + pinName(_enable_pin).c_str(), _pwm_freq, _pwm_precision, laser_mode->getStringValue()); // the current mode use_delays = false; // this will override the value set in Spindle::PWM::init() } + + // Get the GPIO from the machine definition + void Laser::get_pins_and_settings() { + // setup all the pins + +#ifdef LASER_OUTPUT_PIN + _output_pin = LASER_OUTPUT_PIN; +#else + _output_pin = UNDEFINED_PIN; +#endif + + _invert_pwm = spindle_output_invert->get(); + +#ifdef LASER_ENABLE_PIN + _enable_pin = LASER_ENABLE_PIN; +#else + _enable_pin = UNDEFINED_PIN; +#endif + + if (_output_pin == UNDEFINED_PIN) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: LASER_OUTPUT_PIN not defined"); + return; // We cannot continue without the output pin + } + + _off_with_zero_speed = spindle_enbl_off_with_zero_speed->get(); + + _direction_pin = UNDEFINED_PIN; + is_reversable = false; + + _pwm_freq = spindle_pwm_freq->get(); + _pwm_precision = calc_pwm_precision(_pwm_freq); // detewrmine the best precision + _pwm_period = (1 << _pwm_precision); + + // pre-caculate some PWM count values + _pwm_off_value = 0; + _pwm_min_value = 0; + _pwm_max_value = _pwm_period; + + _min_rpm = 0; + _max_rpm = laser_full_power->get(); + + _piecewide_linear = false; + + _pwm_chan_num = 0; // Channel 0 is reserved for spindle use + } + + void Laser::deinit() { + stop(); +#ifdef LASER_OUTPUT_PIN + pinMode(LASER_OUTPUT_PIN, INPUT); +#endif + +#ifdef LASER_ENABLE_PIN + pinMode(LASER_ENABLE_PIN, INPUT); +#endif + } } diff --git a/Grbl_Esp32/src/Spindles/Laser.h b/Grbl_Esp32/src/Spindles/Laser.h index 220770b0..87673f62 100644 --- a/Grbl_Esp32/src/Spindles/Laser.h +++ b/Grbl_Esp32/src/Spindles/Laser.h @@ -36,6 +36,8 @@ namespace Spindles { bool isRateAdjusted() override; void config_message() override; + void get_pins_and_settings() override; + void deinit() override; virtual ~Laser() {} }; diff --git a/Grbl_Esp32/src/Spindles/NullSpindle.cpp b/Grbl_Esp32/src/Spindles/NullSpindle.cpp index c1266019..ddcdb7a2 100644 --- a/Grbl_Esp32/src/Spindles/NullSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/NullSpindle.cpp @@ -40,5 +40,5 @@ namespace Spindles { } SpindleState Null::get_state() { return _current_state; } void Null::stop() {} - void Null::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "No spindle"); } + void Null::config_message() { grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "No spindle"); } } diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp index 3e71ee7b..1ade8aa8 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp @@ -34,12 +34,11 @@ namespace Spindles { get_pins_and_settings(); if (_output_pin == UNDEFINED_PIN) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin not defined"); return; // We cannot continue without the output pin } if (_output_pin >= I2S_OUT_PIN_BASE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin %s cannot do PWM", pinName(_output_pin).c_str()); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle output pin %s cannot do PWM", pinName(_output_pin).c_str()); return; } @@ -80,6 +79,11 @@ namespace Spindles { _direction_pin = UNDEFINED_PIN; #endif + if (_output_pin == UNDEFINED_PIN) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: SPINDLE_OUTPUT_PIN not defined"); + return; // We cannot continue without the output pin + } + is_reversable = (_direction_pin != UNDEFINED_PIN); _pwm_freq = spindle_pwm_freq->get(); @@ -87,7 +91,7 @@ namespace Spindles { _pwm_period = (1 << _pwm_precision); if (spindle_pwm_min_value->get() > spindle_pwm_min_value->get()) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle min pwm is greater than max. Check $35 and $36"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle min pwm is greater than max. Check $35 and $36"); } // pre-caculate some PWM count values @@ -117,8 +121,6 @@ namespace Spindles { return rpm; } - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "set_rpm(%d)", rpm); - // apply override rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (uint8_t percent) @@ -134,7 +136,7 @@ namespace Spindles { if (_piecewide_linear) { //pwm_value = piecewise_linear_fit(rpm); TODO pwm_value = 0; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Linear fit not implemented yet."); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Linear fit not implemented yet."); } else { if (rpm == 0) { @@ -159,18 +161,14 @@ namespace Spindles { sys.spindle_speed = 0; stop(); if (use_delays && (_current_state != state)) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinDown Start "); mc_dwell(spindle_delay_spindown->get()); - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinDown Done"); } } else { set_dir_pin(state == SpindleState::Cw); set_rpm(rpm); set_enable_pin(state != SpindleState::Disable); // must be done after setting rpm for enable features to work if (use_delays && (_current_state != state)) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinUp Start %d", rpm); mc_dwell(spindle_delay_spinup->get()); - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinUp Done"); } } @@ -197,7 +195,7 @@ namespace Spindles { // prints the startup message of the spindle config void PWM::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "PWM spindle Output:%s, Enbl:%s, Dir:%s, Freq:%dHz, Res:%dbits", pinName(_output_pin).c_str(), @@ -223,8 +221,6 @@ namespace Spindles { duty = (1 << _pwm_precision) - duty; } - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "set_output(%d)", duty); - ledcWrite(_pwm_chan_num, duty); } @@ -262,4 +258,18 @@ namespace Spindles { return precision - 1; } + + void PWM::deinit() { + stop(); +#ifdef SPINDLE_OUTPUT_PIN + pinMode(SPINDLE_OUTPUT_PIN, INPUT); +#endif +#ifdef SPINDLE_ENABLE_PIN + pinMode(SPINDLE_ENABLE_PIN, INPUT); +#endif + +#ifdef SPINDLE_DIR_PIN + pinMode(SPINDLE_DIR_PIN, INPUT); +#endif + } } diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.h b/Grbl_Esp32/src/Spindles/PWMSpindle.h index cb306a1d..07ffdf25 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.h +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.h @@ -65,8 +65,9 @@ namespace Spindles { virtual void set_dir_pin(bool Clockwise); virtual void set_output(uint32_t duty); virtual void set_enable_pin(bool enable_pin); + virtual void deinit(); - void get_pins_and_settings(); - uint8_t calc_pwm_precision(uint32_t freq); + virtual void get_pins_and_settings(); + uint8_t calc_pwm_precision(uint32_t freq); }; } diff --git a/Grbl_Esp32/src/Spindles/RelaySpindle.cpp b/Grbl_Esp32/src/Spindles/RelaySpindle.cpp index eadeb918..41762d4c 100644 --- a/Grbl_Esp32/src/Spindles/RelaySpindle.cpp +++ b/Grbl_Esp32/src/Spindles/RelaySpindle.cpp @@ -46,7 +46,7 @@ namespace Spindles { // prints the startup message of the spindle config void Relay ::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Relay spindle Output:%s, Enbl:%s, Dir:%s", pinName(_output_pin).c_str(), diff --git a/Grbl_Esp32/src/Spindles/Spindle.cpp b/Grbl_Esp32/src/Spindles/Spindle.cpp index 7db5edf2..d0ec75a4 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.cpp +++ b/Grbl_Esp32/src/Spindles/Spindle.cpp @@ -100,6 +100,8 @@ namespace Spindles { protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed. set_state(state, rpm); } + + void Spindle::deinit() { stop(); } } Spindles::Spindle* spindle; diff --git a/Grbl_Esp32/src/Spindles/Spindle.h b/Grbl_Esp32/src/Spindles/Spindle.h index d977fa86..5fd37e5b 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.h +++ b/Grbl_Esp32/src/Spindles/Spindle.h @@ -66,6 +66,7 @@ namespace Spindles { virtual void config_message() = 0; virtual bool isRateAdjusted(); virtual void sync(SpindleState state, uint32_t rpm); + virtual void deinit(); virtual ~Spindle() {} diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index 3a1305a6..a7a2636a 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -49,6 +49,7 @@ const int VFD_RS485_POLL_RATE = 200; // in milliseconds between comman namespace Spindles { QueueHandle_t VFD::vfd_cmd_queue = nullptr; TaskHandle_t VFD::vfd_cmdTaskHandle = nullptr; + bool VFD::task_active = true; // The communications task void VFD::vfd_cmd_task(void* pvParameters) { @@ -60,6 +61,12 @@ namespace Spindles { uint8_t rx_message[VFD_RS485_MAX_MSG_SIZE]; while (true) { + if (!task_active) { + uart_driver_delete(VFD_RS485_UART_PORT); + xQueueReset(vfd_cmd_queue); + vTaskDelete(NULL); + } + response_parser parser = nullptr; next_cmd.msg[0] = VFD_RS485_ADDR; // Always default to this @@ -158,7 +165,7 @@ namespace Spindles { // Not succesful! Now what? unresponsive = true; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); } } else { #ifdef VFD_DEBUG_MODE @@ -167,18 +174,18 @@ namespace Spindles { if (read_length != 0) { if (rx_message[0] != VFD_RS485_ADDR) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 received message from other modbus device"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 received message from other modbus device"); } else if (read_length != next_cmd.rx_length) { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 received message of unexpected length; expected %d, got %d", int(next_cmd.rx_length), int(read_length)); } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 CRC check failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 CRC check failed"); } } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 No response"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 No response"); } #endif @@ -193,9 +200,9 @@ namespace Spindles { if (retry_count == MAX_RETRIES) { if (!unresponsive) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); if (next_cmd.critical) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); sys_rt_exec_alarm = ExecAlarm::SpindleControl; } unresponsive = true; @@ -218,12 +225,12 @@ namespace Spindles { void VFD::init() { vfd_ok = false; // initialize - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Initializing RS485 VFD spindle"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Initializing RS485 VFD spindle"); // fail if required items are not defined if (!get_pins_and_settings()) { vfd_ok = false; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD spindle errors"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD spindle errors"); return; } @@ -246,38 +253,37 @@ namespace Spindles { uart_config.rx_flow_ctrl_thresh = 122; if (uart_param_config(VFD_RS485_UART_PORT, &uart_config) != ESP_OK) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart parameters failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart parameters failed"); return; } if (uart_set_pin(VFD_RS485_UART_PORT, _txd_pin, _rxd_pin, _rts_pin, UART_PIN_NO_CHANGE) != ESP_OK) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart pin config failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart pin config failed"); return; } if (uart_driver_install(VFD_RS485_UART_PORT, VFD_RS485_BUF_SIZE * 2, 0, 0, NULL, 0) != ESP_OK) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart driver install failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart driver install failed"); return; } if (uart_set_mode(VFD_RS485_UART_PORT, UART_MODE_RS485_HALF_DUPLEX) != ESP_OK) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart set half duplex failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart set half duplex failed"); return; } // Initialization is complete, so now it's okay to run the queue task: - if (!_task_running) { // init can happen many times, we only want to start one task + task_active = true; + if (vfd_cmd_queue != nullptr) { vfd_cmd_queue = xQueueCreate(VFD_RS485_QUEUE_SIZE, sizeof(ModbusCommand)); - xTaskCreatePinnedToCore(vfd_cmd_task, // task - "vfd_cmdTaskHandle", // name for task - 2048, // size of task stack - this, // parameters - 1, // priority - &vfd_cmdTaskHandle, - 0 // core - ); - _task_running = true; - } + } + xTaskCreatePinnedToCore(vfd_cmd_task, // task + "vfd_cmdTaskHandle", // name for task + 2048, // size of task stack + this, // parameters + 1, // priority + &vfd_cmdTaskHandle, + 1); is_reversable = true; // these VFDs are always reversable use_delays = true; @@ -299,26 +305,26 @@ namespace Spindles { #ifdef VFD_RS485_TXD_PIN _txd_pin = VFD_RS485_TXD_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Undefined VFD_RS485_TXD_PIN"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Undefined VFD_RS485_TXD_PIN"); pins_settings_ok = false; #endif #ifdef VFD_RS485_RXD_PIN _rxd_pin = VFD_RS485_RXD_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Undefined VFD_RS485_RXD_PIN"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Undefined VFD_RS485_RXD_PIN"); pins_settings_ok = false; #endif #ifdef VFD_RS485_RTS_PIN _rts_pin = VFD_RS485_RTS_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Undefined VFD_RS485_RTS_PIN"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Undefined VFD_RS485_RTS_PIN"); pins_settings_ok = false; #endif if (laser_mode->get()) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD spindle disabled in laser mode. Set $GCode/LaserMode=Off and restart"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD spindle disabled in laser mode. Set $GCode/LaserMode=Off and restart"); pins_settings_ok = false; } @@ -329,7 +335,7 @@ namespace Spindles { } void VFD::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD RS485 Tx:%s Rx:%s RTS:%s", pinName(_txd_pin).c_str(), @@ -382,7 +388,7 @@ namespace Spindles { if (mode == SpindleState::Disable) { if (!xQueueReset(vfd_cmd_queue)) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD spindle off, queue could not be reset"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD spindle off, queue could not be reset"); } } @@ -390,7 +396,7 @@ namespace Spindles { _current_state = mode; if (xQueueSend(vfd_cmd_queue, &mode_cmd, 0) != pdTRUE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD Queue Full"); } return true; @@ -402,7 +408,7 @@ namespace Spindles { } #ifdef VFD_DEBUG_MODE - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); #endif // apply override @@ -433,7 +439,7 @@ namespace Spindles { rpm_cmd.critical = false; if (xQueueSend(vfd_cmd_queue, &rpm_cmd, 0) != pdTRUE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD Queue Full"); } return rpm; @@ -465,4 +471,11 @@ namespace Spindles { return crc; } + + void VFD::deinit() { + _current_state = SpindleState::Disable; + _current_rpm = 0; + + task_active = false; + } } diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.h b/Grbl_Esp32/src/Spindles/VFDSpindle.h index 1c852292..b2b091ec 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.h +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.h @@ -39,13 +39,18 @@ namespace Spindles { uint32_t _current_rpm = 0; bool _task_running = false; bool vfd_ok = true; - + + static bool task_active; static QueueHandle_t vfd_cmd_queue; static TaskHandle_t vfd_cmdTaskHandle; static void vfd_cmd_task(void* pvParameters); static uint16_t ModRTU_CRC(uint8_t* buf, int msg_len); + void deinit() override; + + + protected: struct ModbusCommand { bool critical; // TODO SdB: change into `uint8_t critical : 1;`: We want more flags... diff --git a/Grbl_Esp32/src/WebUI/BTConfig.cpp b/Grbl_Esp32/src/WebUI/BTConfig.cpp index 05a7816e..85a2a7e6 100644 --- a/Grbl_Esp32/src/WebUI/BTConfig.cpp +++ b/Grbl_Esp32/src/WebUI/BTConfig.cpp @@ -86,6 +86,9 @@ namespace WebUI { bool BTConfig::isBTnameValid(const char* hostname) { //limited size + if (!hostname) { + return true; + } char c; // length is checked automatically by string setting //only letter and digit diff --git a/Grbl_Esp32/src/WebUI/Commands.cpp b/Grbl_Esp32/src/WebUI/Commands.cpp index 7cfdf003..d59ee19c 100644 --- a/Grbl_Esp32/src/WebUI/Commands.cpp +++ b/Grbl_Esp32/src/WebUI/Commands.cpp @@ -43,6 +43,9 @@ namespace WebUI { } bool COMMANDS::isLocalPasswordValid(char* password) { + if (!password) { + return true; + } char c; //limited size if ((strlen(password) > MAX_LOCAL_PASSWORD_LENGTH) || (strlen(password) < MIN_LOCAL_PASSWORD_LENGTH)) { diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index b9a34b8c..f9a00fc4 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -1085,7 +1085,7 @@ namespace WebUI { MAX_NOTIFICATION_TOKEN_LENGTH, NULL); notification_type = - new EnumSetting("Notification type", WEBSET, WA, NULL, "Notification/Type", DEFAULT_NOTIFICATION_TYPE, ¬ificationOptions); + new EnumSetting("Notification type", WEBSET, WA, NULL, "Notification/Type", DEFAULT_NOTIFICATION_TYPE, ¬ificationOptions, NULL); #endif #ifdef ENABLE_AUTHENTICATION user_password = new StringSetting("User password", @@ -1121,16 +1121,16 @@ namespace WebUI { #ifdef WIFI_OR_BLUETOOTH // user+ to get, admin to set - wifi_radio_mode = new EnumSetting("Radio mode", WEBSET, WA, "ESP110", "Radio/Mode", DEFAULT_RADIO_MODE, &radioEnabledOptions); + wifi_radio_mode = new EnumSetting("Radio mode", WEBSET, WA, "ESP110", "Radio/Mode", DEFAULT_RADIO_MODE, &radioEnabledOptions, NULL); #endif #ifdef ENABLE_WIFI telnet_port = new IntSetting( "Telnet Port", WEBSET, WA, "ESP131", "Telnet/Port", DEFAULT_TELNETSERVER_PORT, MIN_TELNET_PORT, MAX_TELNET_PORT, NULL); - telnet_enable = new EnumSetting("Telnet Enable", WEBSET, WA, "ESP130", "Telnet/Enable", DEFAULT_TELNET_STATE, &onoffOptions); + telnet_enable = new EnumSetting("Telnet Enable", WEBSET, WA, "ESP130", "Telnet/Enable", DEFAULT_TELNET_STATE, &onoffOptions, NULL); http_port = new IntSetting("HTTP Port", WEBSET, WA, "ESP121", "Http/Port", DEFAULT_WEBSERVER_PORT, MIN_HTTP_PORT, MAX_HTTP_PORT, NULL); - http_enable = new EnumSetting("HTTP Enable", WEBSET, WA, "ESP120", "Http/Enable", DEFAULT_HTTP_STATE, &onoffOptions); + http_enable = new EnumSetting("HTTP Enable", WEBSET, WA, "ESP120", "Http/Enable", DEFAULT_HTTP_STATE, &onoffOptions, NULL); wifi_hostname = new StringSetting("Hostname", WEBSET, WA, @@ -1158,7 +1158,7 @@ namespace WebUI { wifi_sta_netmask = new IPaddrSetting("Station Static Mask", WEBSET, WA, NULL, "Sta/Netmask", DEFAULT_STA_MK, NULL); wifi_sta_gateway = new IPaddrSetting("Station Static Gateway", WEBSET, WA, NULL, "Sta/Gateway", DEFAULT_STA_GW, NULL); wifi_sta_ip = new IPaddrSetting("Station Static IP", WEBSET, WA, NULL, "Sta/IP", DEFAULT_STA_IP, NULL); - wifi_sta_mode = new EnumSetting("Station IP Mode", WEBSET, WA, "ESP102", "Sta/IPMode", DEFAULT_STA_IP_MODE, &staModeOptions); + wifi_sta_mode = new EnumSetting("Station IP Mode", WEBSET, WA, "ESP102", "Sta/IPMode", DEFAULT_STA_IP_MODE, &staModeOptions, NULL); // no get, admin to set wifi_sta_password = new StringSetting("Station Password", WEBSET, diff --git a/Grbl_Esp32/src/WebUI/WifiConfig.cpp b/Grbl_Esp32/src/WebUI/WifiConfig.cpp index d918e7c7..e42c0c05 100644 --- a/Grbl_Esp32/src/WebUI/WifiConfig.cpp +++ b/Grbl_Esp32/src/WebUI/WifiConfig.cpp @@ -115,6 +115,9 @@ namespace WebUI { bool WiFiConfig::isHostnameValid(const char* hostname) { //limited size + if (!hostname) { + return true; + } char c; // length is checked automatically by string setting //only letter and digit @@ -139,6 +142,9 @@ namespace WebUI { //char c; // length is checked automatically by string setting //only printable + if (!ssid) { + return true; + } for (int i = 0; i < strlen(ssid); i++) { if (!isPrintable(ssid[i])) { return false; @@ -152,6 +158,9 @@ namespace WebUI { */ bool WiFiConfig::isPasswordValid(const char* password) { + if (!password) { + return true; + } if (strlen(password) == 0) { return true; //open network }