diff --git a/Grbl_Esp32/BTconfig.cpp b/Grbl_Esp32/BTconfig.cpp index 515f382b..f761e552 100644 --- a/Grbl_Esp32/BTconfig.cpp +++ b/Grbl_Esp32/BTconfig.cpp @@ -20,14 +20,11 @@ #ifdef ARDUINO_ARCH_ESP32 -#include "config.h" +#include "grbl.h" #ifdef ENABLE_BLUETOOTH -#include #include "BluetoothSerial.h" #include "BTconfig.h" -#include "commands.h" -#include "report.h" BTConfig bt_config; BluetoothSerial SerialBT; @@ -93,8 +90,7 @@ const char* BTConfig::info() { bool BTConfig::isBTnameValid(const char* hostname) { //limited size char c; - if (strlen(hostname) > MAX_BTNAME_LENGTH || strlen(hostname) < MIN_BTNAME_LENGTH) - return false; + // length is checked automatically by string setting //only letter and digit for (int i = 0; i < strlen(hostname); i++) { c = hostname[i]; @@ -116,16 +112,10 @@ const char* BTConfig::device_address() { * begin WiFi setup */ void BTConfig::begin() { - Preferences prefs; //stop active services end(); - prefs.begin(NAMESPACE, true); - //Get hostname - String defV = DEFAULT_BT_NAME; - _btname = prefs.getString(BT_NAME_ENTRY, defV); - int8_t wifiMode = prefs.getChar(ESP_RADIO_MODE, DEFAULT_RADIO_MODE); - prefs.end(); - if (wifiMode == ESP_BT) { + _btname = bt_name->get(); + if (wifi_radio_mode->get() == ESP_BT) { if (!SerialBT.begin(_btname)) report_status_message(STATUS_BT_FAIL_BEGIN, CLIENT_ALL); else { @@ -146,22 +136,9 @@ void BTConfig::end() { * Reset ESP */ void BTConfig::reset_settings() { - Preferences prefs; - prefs.begin(NAMESPACE, false); - String sval; - int8_t bbuf; - bool error = false; - sval = DEFAULT_BT_NAME; - if (prefs.putString(BT_NAME_ENTRY, sval) == 0) - error = true; - bbuf = DEFAULT_RADIO_MODE; - if (prefs.putChar(ESP_RADIO_MODE, bbuf) == 0) - error = true; - prefs.end(); - if (error) - grbl_send(CLIENT_ALL, "[MSG:BT reset error]\r\n"); - else - grbl_send(CLIENT_ALL, "[MSG:BT reset done]\r\n"); + bt_name->setDefault(); + wifi_radio_mode->setDefault(); + grbl_send(CLIENT_ALL, "[MSG:BT reset done]\r\n"); } /** diff --git a/Grbl_Esp32/BTconfig.h b/Grbl_Esp32/BTconfig.h index 1a2a7eed..4bba20b5 100644 --- a/Grbl_Esp32/BTconfig.h +++ b/Grbl_Esp32/BTconfig.h @@ -21,9 +21,6 @@ #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it #endif -//Preferences entries -#define BT_NAME_ENTRY "BT_NAME" - //defaults values #define DEFAULT_BT_NAME "btgrblesp" diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp index 53067a2b..b111a20f 100644 --- a/Grbl_Esp32/Custom/atari_1020.cpp +++ b/Grbl_Esp32/Custom/atari_1020.cpp @@ -134,9 +134,9 @@ void atari_home_task(void* pvParameters) { if (digitalRead(REED_SW_PIN) == 0) { // see if reed switch is grounded inputBuffer.push("G4P0.1\n"); // dramtic pause - sys_position[X_AXIS] = ATARI_HOME_POS * settings.steps_per_mm[X_AXIS]; + sys_position[X_AXIS] = ATARI_HOME_POS * axis_settings[X_AXIS]->steps_per_mm->get(); sys_position[Y_AXIS] = 0.0; - sys_position[Z_AXIS] = 1.0 * settings.steps_per_mm[Y_AXIS]; + sys_position[Z_AXIS] = 1.0 * axis_settings[Y_AXIS]->steps_per_mm->get(); gc_sync_position(); plan_sync_position(); sprintf(gcode_line, "G90G0X%3.2f\r", ATARI_PAPER_WIDTH); // alway return to right side to reduce home travel stalls diff --git a/Grbl_Esp32/Custom/esp32_printer_controller.cpp b/Grbl_Esp32/Custom/esp32_printer_controller.cpp new file mode 100644 index 00000000..c232a068 --- /dev/null +++ b/Grbl_Esp32/Custom/esp32_printer_controller.cpp @@ -0,0 +1,197 @@ +/* + custom_code_template.cpp (copy and use your machine name) + Part of Grbl_ESP32 + + copyright (c) 2020 - Bart Dring. This file was intended for use on the ESP32 + + ...add your date and name here. + + CPU. Do not use this with Grbl for atMega328P + + Grbl_ESP32 is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl_ESP32 is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . + + ======================================================================= + +This is a template for user-defined C++ code functions. Grbl can be +configured to call some optional functions, enabled by #define statements +in the machine definition .h file. Implement the functions thus enabled +herein. The possible functions are listed and described below. + +To use this file, copy it to a name of your own choosing, and also copy +Machines/template.h to a similar name. + +Example: +Machines/my_machine.h +Custom/my_machine.cpp + +Edit machine.h to include your Machines/my_machine.h file + +Edit Machines/my_machine.h according to the instructions therein. + +Fill in the function definitions below for the functions that you have +enabled with USE_ defines in Machines/my_machine.h + +=============================================================================== + +*/ + +#ifdef USE_MACHINE_INIT +/* +machine_init() is called when Grbl_ESP32 first starts. You can use it to do any +special things your machine needs at startup. +*/ +#define STEPPERS_DISABLE_PIN_X 138 +#define STEPPERS_DISABLE_PIN_Y 134 +#define STEPPERS_DISABLE_PIN_Z 131 +#define STEPPERS_DISABLE_PIN_A 139 + +#define FAN1_PIN 13 +#define FAN2_PIN 142 +#define FAN3_PIN 143 + +#define BED_PIN 4 +#define NOZZLE_PIN 2 + +void machine_init() +{ + // Enable steppers + digitalWrite(STEPPERS_DISABLE_PIN_X, LOW); // enable + digitalWrite(STEPPERS_DISABLE_PIN_Y, LOW); // enable + digitalWrite(STEPPERS_DISABLE_PIN_Z, LOW); // enable + digitalWrite(STEPPERS_DISABLE_PIN_A, LOW); // enable + + // digitalWrite(FAN1_PIN, LOW); // comment out for JTAG debugging + + digitalWrite(FAN2_PIN, LOW); // disable + digitalWrite(FAN3_PIN, LOW); // disable + + digitalWrite(BED_PIN, LOW); // disable + digitalWrite(NOZZLE_PIN, LOW); // disable +} +#endif + +#ifdef USE_CUSTOM_HOMING +/* + user_defined_homing() is called at the begining of the normal Grbl_ESP32 homing + sequence. If user_defined_homing() returns false, the rest of normal Grbl_ESP32 + homing is skipped if it returns false, other normal homing continues. For + example, if you need to manually prep the machine for homing, you could implement + user_defined_homing() to wait for some button to be pressed, then return true. +*/ +bool user_defined_homing() +{ + // True = done with homing, false = continue with normal Grbl_ESP32 homing + return true; +} +#endif + +#ifdef USE_KINEMATICS +/* + Inverse Kinematics converts X,Y,Z cartesian coordinate to the steps + on your "joint" motors. It requires the following three functions: +*/ + +/* + inverse_kinematics() looks at incoming move commands and modifies + them before Grbl_ESP32 puts them in the motion planner. + + Grbl_ESP32 processes arcs by converting them into tiny little line segments. + Kinematics in Grbl_ESP32 works the same way. Search for this function across + Grbl_ESP32 for examples. You are basically converting cartesian X,Y,Z... targets to + + target = an N_AXIS array of target positions (where the move is supposed to go) + pl_data = planner data (see the definition of this type to see what it is) + position = an N_AXIS array of where the machine is starting from for this move +*/ +void inverse_kinematics(float *target, plan_line_data_t *pl_data, float *position) +{ + // this simply moves to the target. Replace with your kinematics. + mc_line(target, pl_data); +} + +/* + kinematics_pre_homing() is called before normal homing + You can use it to do special homing or just to set stuff up + + cycle_mask is a bit mask of the axes being homed this time. +*/ +bool kinematics_pre_homing(uint8_t cycle_mask)) +{ + return false; // finish normal homing cycle +} + +/* + kinematics_post_homing() is called at the end of normal homing +*/ +void kinematics_post_homing() +{ +} +#endif + +#ifdef USE_FWD_KINEMATIC +/* + The status command uses forward_kinematics() to convert + your motor positions to cartesian X,Y,Z... coordinates. + + Convert the N_AXIS array of motor positions to cartesian in your code. +*/ +void forward_kinematics(float *position) +{ + // position[X_AXIS] = + // position[Y_AXIS] = +} +#endif + +#ifdef USE_TOOL_CHANGE +/* + user_tool_change() is called when tool change gcode is received, + to perform appropriate actions for your machine. +*/ +void user_tool_change(uint8_t new_tool) +{ +} +#endif + +#if defined(MACRO_BUTTON_0_PIN) || defined(MACRO_BUTTON_1_PIN) || defined(MACRO_BUTTON_2_PIN) +/* + options. user_defined_macro() is called with the button number to + perform whatever actions you choose. +*/ +void user_defined_macro(uint8_t index) +{ +} +#endif + +#ifdef USE_M30 +/* + user_m30() is called when an M30 gcode signals the end of a gcode file. +*/ +void user_m30() +{ +} +#endif + +#ifdef USE_MACHINE_TRINAMIC_INIT +/* + machine_triaminic_setup() replaces the normal setup of trinamic + drivers with your own code. For example, you could setup StallGuard +*/ +void machine_trinamic_setup() +{ +} +#endif + +// If you add any additional functions specific to your machine that +// require calls from common code, guard their calls in the common code with +// #ifdef USE_WHATEVER and add function prototypes (also guarded) to grbl.h diff --git a/Grbl_Esp32/Custom/polar_coaster.cpp b/Grbl_Esp32/Custom/polar_coaster.cpp index 1cd00c05..036c283c 100644 --- a/Grbl_Esp32/Custom/polar_coaster.cpp +++ b/Grbl_Esp32/Custom/polar_coaster.cpp @@ -65,10 +65,6 @@ static float last_radius = 0; // return false to complete normal home // return true to exit normal homing bool kinematics_pre_homing(uint8_t cycle_mask) { - // cycle mask not used for polar coaster - // zero the axes that are not homed - sys_position[Y_AXIS] = 0.0f; - sys_position[Z_AXIS] = SERVO_Z_RANGE_MAX * settings.steps_per_mm[Z_AXIS]; // Move pen up. return false; // finish normal homing cycle } diff --git a/Grbl_Esp32/Grbl_Esp32.ino b/Grbl_Esp32/Grbl_Esp32.ino index 8c7d757a..26431157 100644 --- a/Grbl_Esp32/Grbl_Esp32.ino +++ b/Grbl_Esp32/Grbl_Esp32.ino @@ -22,6 +22,7 @@ #include "WiFi.h" #include "Spindles/SpindleClass.cpp" +#include "Motors/MotorClass.cpp" // Declare system global variable structure system_t sys; @@ -41,6 +42,9 @@ Spindle *spindle; void setup() { +#ifdef USE_I2S_OUT + i2s_out_init(); // The I2S out must be initialized before it can access the expanded GPIO port +#endif WiFi.persistent(false); WiFi.disconnect(true); WiFi.enableSTA(false); @@ -60,6 +64,7 @@ void setup() { #endif settings_init(); // Load Grbl settings from EEPROM stepper_init(); // Configure stepper pins and interrupt timers + init_motors(); system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files) memset(sys_position, 0, sizeof(sys_position)); // Clear machine position. #ifdef USE_PEN_SERVO @@ -89,9 +94,9 @@ void setup() { // not after disabling the alarm locks. Prevents motion startup blocks from crashing into // things uncontrollably. Very bad. #ifdef HOMING_INIT_LOCK - if (bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)) sys.state = STATE_ALARM; + if (homing_enable->get()) sys.state = STATE_ALARM; #endif - spindle_select(SPINDLE_TYPE); + spindle_select(); #ifdef ENABLE_WIFI wifi_config.begin(); #endif diff --git a/Grbl_Esp32/JSONencoder.cpp b/Grbl_Esp32/JSONencoder.cpp new file mode 100644 index 00000000..19fb50a0 --- /dev/null +++ b/Grbl_Esp32/JSONencoder.cpp @@ -0,0 +1,171 @@ +// Class for creating JSON-encoded strings. + +#include "grbl.h" + +#include "JSONencoder.h" + +// Constructor. If _pretty is true, newlines are +// inserted into the JSON string for easy reading. +JSONencoder::JSONencoder(bool pretty) : + pretty(pretty), + level(0), + str("") +{ + count[level] = 0; +} + +// Private function to add commas between +// elements as needed, omitting the comma +// before the first element in a list. +// If pretty-printing is enabled, a newline +// is added after the comma. +void JSONencoder::comma_line() { + if (count[level]) { + add(','); + line(); + } + count[level]++; +} + +// Private function to add commas between +// elements as needed, omitting the comma +// before the first element in a list. +void JSONencoder::comma() { + if (count[level]) { + add(','); + } + count[level]++; +} + +// Private function to add a name enclosed with quotes. +void JSONencoder::quoted(const char *s) +{ + add('"'); + str.concat(s); + add('"'); +} + +// Private function to increment the nesting level. +// It's necessary to account for the level in order +// to handle commas properly, as each level must +// know when to omit the comma. +void JSONencoder::inc_level() { + if (++level == MAX_JSON_LEVEL) { + --level; + } + count[level] = 0; +} + +// Private function to increment the nesting level. +void JSONencoder::dec_level() { + --level; +} + +// Private function to implement pretty-printing +void JSONencoder::line() { + if (pretty) { + add('\n'); + for (int i=0; i < 2*level; i++) { + add(' '); + } + } +} + +// Constructor that supplies a default falue for "pretty" +JSONencoder::JSONencoder() : + JSONencoder(false) +{ } + + +// Begins the JSON encoding process, creating an unnamed object +void JSONencoder::begin() { + begin_object(); +} + +// Finishes the JSON encoding process, closing the unnamed object +// and returning the encoded string +String JSONencoder::end() { + end_object(); + return str; +} + +// Starts a member element. +void JSONencoder::begin_member(const char *tag) { + comma_line(); + quoted(tag); + add(':'); +} + +// Starts an array with "tag":[ +void JSONencoder::begin_array(const char *tag) { + begin_member(tag); + add('['); + inc_level(); + line(); +} + +// Ends an array with ] +void JSONencoder::end_array() { + dec_level(); + line(); + add(']'); +} + +// Starts an object with {. +// If you need a named object you must call begin_member() first. +void JSONencoder::begin_object() { + comma_line(); + add('{'); + inc_level(); +} + +// Ends an object with }. +void JSONencoder::end_object() { + dec_level(); + if (count[level+1] > 1) { + line(); + } + add('}'); +} + +// Creates a "tag":"value" member from a C-style string +void JSONencoder::member(const char *tag, const char *value) { + begin_member(tag); + quoted(value); +} + +// Creates a "tag":"value" member from an Arduino string +void JSONencoder::member(const char *tag, String value) { + begin_member(tag); + quoted(value.c_str()); +} + +// Creates a "tag":"value" member from an integer +void JSONencoder::member(const char *tag, int value) { + member(tag, String(value)); +} + +// Creates an Esp32_WebUI configuration item specification from +// a value passed in as a C-style string. +void JSONencoder::begin_webui(const char *p, const char *help, const char *type, const char *val) { + begin_object(); + member("F", "network"); + member("P", p); + member("H", help); + member("T", type); + member("V", val); +} + +// Creates an Esp32_WebUI configuration item specification from +// an integer value. +void JSONencoder::begin_webui(const char *p, const char *help, const char *type, int val) { + begin_webui(p, help, type, String(val).c_str()); +} + +// Creates an Esp32_WebUI configuration item specification from +// a C-style string value, with additional min and max arguments. +void JSONencoder::begin_webui(const char *p, const char *help, const char *type, const char *val, int min, int max) { + begin_webui(p, help, type, val); + member("S", max); + member("M", min); +} diff --git a/Grbl_Esp32/JSONencoder.h b/Grbl_Esp32/JSONencoder.h new file mode 100644 index 00000000..eae79625 --- /dev/null +++ b/Grbl_Esp32/JSONencoder.h @@ -0,0 +1,76 @@ +// Class for creating JSON-encoded strings. + +#pragma once +#define MAX_JSON_LEVEL 16 +class JSONencoder { + private: + bool pretty; + int level; + String str; + int count[MAX_JSON_LEVEL]; + void add(char c) { str += c; } + void comma_line(); + void comma(); + void quoted(const char *s); + void inc_level(); + void dec_level(); + void line(); + public: + // Constructor; set _pretty true for pretty printing + JSONencoder(bool pretty); + // If you don't set _pretty it defaults to false + JSONencoder(); + + // begin() starts the encoding process. + void begin(); + + // end() returns the encoded string + String end(); + + // member() creates a "tag":"value" element + void member(const char *tag, const char *value); + void member(const char *tag, String value); + void member(const char *tag, int value); + + // begin_array() starts a "tag":[ array element + void begin_array(const char *tag); + + // end_array() closes the array with ] + void end_array(); + + // begin_object() starts an object with { + void begin_object(); + + // end_object() closes the object with } + void end_object(); + + // begin_member() starts the creation of a member. + // The only case where you need to use it directly + // is when you want a member whose value is an object. + void begin_member(const char *tag); + + // The begin_webui() methods are specific to Esp3D_WebUI + // WebUI sends JSON objects to the UI to generate configuration + // page entries. Each object describes a named setting with a + // type, current value, and a description of the possible values. + // The possible values can either be a minumum and maximum + // integer value, min/max string length, or an enumeration list. + // When the user chooses a value, this command is sent back: + // [ESP401]P=p T=type V=value + // P: parameter name + // T: type + // M: min_val + // S: max_val + // O: options:[ { "name", "value" } ... ] + // V: currentvalue + // H: label + // F: F ("network", used for filtering) + // If M and S are not supplied, they are inferred from type: + // B => -127 .. 255 + // S => 0 .. 255 + // A => 7 .. 15 (0.0.0.0 .. 255.255.255.255) + // I => 0 .. 2^31-1 + void begin_webui(const char *p, const char *help, const char *type, const char *val); + void begin_webui(const char *p, const char *help, const char *type, const int val); + void begin_webui(const char *p, const char *help, const char *type, const char *val, int min, int max); +}; diff --git a/Grbl_Esp32/Machines/3axis_v3.h b/Grbl_Esp32/Machines/3axis_v3.h index 0bfa1429..e95b0d5c 100644 --- a/Grbl_Esp32/Machines/3axis_v3.h +++ b/Grbl_Esp32/Machines/3axis_v3.h @@ -32,7 +32,6 @@ #define Z_STEP_PIN GPIO_NUM_27 #define Z_DIRECTION_PIN GPIO_NUM_33 -#define LIMIT_MASK B111 #define X_LIMIT_PIN GPIO_NUM_2 // labeled X Limit #define Y_LIMIT_PIN GPIO_NUM_4 // labeled Y Limit #define Z_LIMIT_PIN GPIO_NUM_15 // labeled Z Limit diff --git a/Grbl_Esp32/Machines/3axis_v4.h b/Grbl_Esp32/Machines/3axis_v4.h index b3d4be90..475adaa0 100644 --- a/Grbl_Esp32/Machines/3axis_v4.h +++ b/Grbl_Esp32/Machines/3axis_v4.h @@ -32,7 +32,6 @@ #define Z_STEP_PIN GPIO_NUM_27 #define Z_DIRECTION_PIN GPIO_NUM_33 -#define LIMIT_MASK B111 #define X_LIMIT_PIN GPIO_NUM_17 #define Y_LIMIT_PIN GPIO_NUM_4 #define Z_LIMIT_PIN GPIO_NUM_16 @@ -40,12 +39,12 @@ #ifdef HOMING_CYCLE_0 #undef HOMING_CYCLE_0 #endif -#define HOMING_CYCLE_0 (1<. +*/ +#define MACHINE_NAME "6 Pack Controller V1 (StepStick)" + +#ifdef N_AXIS + #undef N_AXIS +#endif +#define N_AXIS 6 + +#ifdef ENABLE_SD_CARD + #undef ENABLE_SD_CARD +#endif + +// === Special Features + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +// Define USE_I2S_OUT_STREAM if buffering is used. +// (there will be a delay between the specified I/O operation and the actual I/O execution) +#define USE_I2S_OUT_STREAM +#undef USE_RMT_STEPS + +#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set + +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 + + +#define STEPPER_X_MS3 I2SO(3) // X_CS +#define STEPPER_Y_MS3 I2SO(6) // Y_CS +#define STEPPER_Z_MS3 I2SO(11) // Z_CS +#define STEPPER_A_MS3 I2SO(14) // A_CS +#define STEPPER_B_MS3 I2SO(19) // B_CS +#define STEPPER_C_MS3 I2SO(22) // C_CS + +#define STEPPER_RESET GPIO_NUM_19 + +#define X_DISABLE_PIN I2SO(0) +#define X_DIRECTION_PIN I2SO(1) +#define X_STEP_PIN I2SO(2) + +#define Y_DIRECTION_PIN I2SO(4) +#define Y_STEP_PIN I2SO(5) +#define Y_DISABLE_PIN I2SO(7) + +#define Z_DISABLE_PIN I2SO(8) +#define Z_DIRECTION_PIN I2SO(9) +#define Z_STEP_PIN I2SO(10) + +#define A_DIRECTION_PIN I2SO(12) +#define A_STEP_PIN I2SO(13) +#define A_DISABLE_PIN I2SO(15) + +#define B_DISABLE_PIN I2SO(16) +#define B_DIRECTION_PIN I2SO(17) +#define B_STEP_PIN I2SO(18) + +#define C_DIRECTION_PIN I2SO(20) +#define C_STEP_PIN I2SO(21) +#define C_DISABLE_PIN I2SO(23) + +#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 +#define B_LIMIT_PIN GPIO_NUM_39 +#define C_LIMIT_PIN GPIO_NUM_36 + +#define PROBE_PIN GPIO_NUM_25 + + +// 0-10v CNC Module in Socket #3 +// Control...Set PD001 to 1 if enable is connected 0 is panel should be used +// Freq...Set PD002 to 0 +// Freq input...Set PD070 to 0 for 0-10V +#define SPINDLE_TYPE SPINDLE_TYPE_10V +#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 +#define SPINDLE_FORWARD_PIN GPIO_NUM_4 +#define SPINDLE_REVERSE_PIN GPIO_NUM_16 + + +// Example 5V output CNC module in socket #3 +/* +#define SPINDLE_TYPE SPINDLE_TYPE_PWM +#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 +#define SPINDLE_ENABLE_PIN GPIO_NUM_4 +#define SPINDLE_DIR_PIN GPIO_NUM_16 +#define COOLANT_MIST_PIN GPIO_NUM_27 +*/ + +/* +// Example (4x) 5V Buffer Output on socket #5 +#define SPINDLE_TYPE SPINDLE_TYPE_RELAY +#define SPINDLE_OUTPUT_PIN I2SO(24) +#define SPINDLE_DIR_PIN I2SO(25) +#define COOLANT_MIST_PIN I2SO(26) +#define COOLANT_FLOOD_PIN I2SO(27) +*/ + +/* +// RS485 In socket #3 +#define SPINDLE_TYPE SPINDLE_TYPE_HUANYANG // only one spindle at a time +#define HUANYANG_TXD_PIN GPIO_NUM_26 +#define HUANYANG_RTS_PIN GPIO_NUM_4 +#define HUANYANG_RXD_PIN GPIO_NUM_16 +*/ + +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/Machines/6_pack_trinamic_V1.h b/Grbl_Esp32/Machines/6_pack_trinamic_V1.h new file mode 100644 index 00000000..038fcff7 --- /dev/null +++ b/Grbl_Esp32/Machines/6_pack_trinamic_V1.h @@ -0,0 +1,105 @@ +/* + 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)" + +#ifdef N_AXIS + #undef N_AXIS +#endif +#define N_AXIS 6 + +// === Special Features + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +// Define USE_I2S_OUT_STREAM if buffering is used. +// (there will be a delay between the specified I/O operation and the actual I/O execution) +#define USE_I2S_OUT_STREAM +#undef USE_RMT_STEPS + +#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 2130 +#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 2130 +#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 2130 +#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 2130 +#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 2130 +#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 + + +// 0-10v CNC Module in Socket #3 +#define SPINDLE_TYPE SPINDLE_TYPE_PWM +#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 +#define SPINDLE_ENABLE_PIN GPIO_NUM_4 +#define SPINDLE_DIR_PIN GPIO_NUM_16 + +#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 +#define B_LIMIT_PIN GPIO_NUM_39 +#define C_LIMIT_PIN GPIO_NUM_36 + +#define PROBE_PIN GPIO_NUM_25 + +#define COOLANT_MIST_PIN GPIO_NUM_2 + +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/Machines/atari_1020.h b/Grbl_Esp32/Machines/atari_1020.h index d2e01270..548cbe65 100644 --- a/Grbl_Esp32/Machines/atari_1020.h +++ b/Grbl_Esp32/Machines/atari_1020.h @@ -28,7 +28,7 @@ along with Grbl_ESP32. If not, see . */ -#define MACHINE_NAME "ATARI_1020" +#define MACHINE_NAME "MACHINE_ATARI_1020" #define CUSTOM_CODE_FILENAME "Custom/atari_1020.cpp" @@ -36,8 +36,6 @@ #undef USE_RMT_STEPS #endif -#define USE_UNIPOLAR - #define X_UNIPOLAR #define X_PIN_PHASE_0 GPIO_NUM_13 #define X_PIN_PHASE_1 GPIO_NUM_21 @@ -56,7 +54,7 @@ #ifdef HOMING_CYCLE_0 #undef HOMING_CYCLE_0 #endif -#define HOMING_CYCLE_0 (1 << X_AXIS) // this 'bot only homes the X axis +#define HOMING_CYCLE_0 bit(X_AXIS) // this 'bot only homes the X axis #ifdef HOMING_CYCLE_1 #undef HOMING_CYCLE_1 #endif @@ -65,7 +63,6 @@ #endif #define REED_SW_PIN GPIO_NUM_17 -#define LIMIT_MASK 0 #ifndef ENABLE_CONTROL_SW_DEBOUNCE #define ENABLE_CONTROL_SW_DEBOUNCE @@ -121,9 +118,9 @@ #define DEFAULT_Y_MAX_RATE 5000.0 // mm/min #define DEFAULT_Z_MAX_RATE 200000.0 // mm/min -#define DEFAULT_X_ACCELERATION (500.0 * 60 * 60) // 10*60*60 mm/min^2 = 10 mm/sec^2 -#define DEFAULT_Y_ACCELERATION (500.0 * 60 * 60) // 10*60*60 mm/min^2 = 10 mm/sec^2 -#define DEFAULT_Z_ACCELERATION (500.0 * 60 * 60) +#define DEFAULT_X_ACCELERATION 500.0 // mm/sec^2. 500 mm/sec^2 = 1800000 mm/min^2 +#define DEFAULT_Y_ACCELERATION 500.0 // mm/sec^2 +#define DEFAULT_Z_ACCELERATION 500.0 // mm/sec^2 #define DEFAULT_X_MAX_TRAVEL 120.0 // mm NOTE: Must be a positive value. #define DEFAULT_Y_MAX_TRAVEL 20000.0 // mm NOTE: Must be a positive value. diff --git a/Grbl_Esp32/Machines/esp32_printer_controller.h b/Grbl_Esp32/Machines/esp32_printer_controller.h new file mode 100644 index 00000000..6938a33a --- /dev/null +++ b/Grbl_Esp32/Machines/esp32_printer_controller.h @@ -0,0 +1,296 @@ +/* + 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 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), +// 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 + +// Grbl_ESP32 use the ESP32's special RMT (IR remote control) hardware +// engine to achieve more precise high step rates than can be done +// in software. That feature is enabled by default, but there are +// some machines that might not want to use it, such as machines that +// do not use ordinary stepper motors. To turn it off, do this: +#undef USE_RMT_STEPS + +// +// I2S (steppers & other output-only pins) +// +#define USE_I2S_OUT +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 +// Define USE_I2S_OUT_STREAM if buffering is used. +// (there will be a delay between the specified I/O operation and the actual I/O execution) +#define I2S_OUT_NUM_BITS 16 +#define USE_I2S_OUT_STREAM + +// === 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/Machines/espduino.h b/Grbl_Esp32/Machines/espduino.h index 2023ccfb..b049f907 100644 --- a/Grbl_Esp32/Machines/espduino.h +++ b/Grbl_Esp32/Machines/espduino.h @@ -50,7 +50,6 @@ #define X_LIMIT_PIN GPIO_NUM_13 #define Y_LIMIT_PIN GPIO_NUM_5 #define Z_LIMIT_PIN GPIO_NUM_19 -#define LIMIT_MASK B111 #define PROBE_PIN GPIO_NUM_39 diff --git a/Grbl_Esp32/Machines/foo_6axis.h b/Grbl_Esp32/Machines/foo_6axis.h index d2222f79..7ce7192d 100644 --- a/Grbl_Esp32/Machines/foo_6axis.h +++ b/Grbl_Esp32/Machines/foo_6axis.h @@ -69,7 +69,6 @@ #define Y_LIMIT_PIN GPIO_NUM_17 #define A_LIMIT_PIN GPIO_NUM_16 #define B_LIMIT_PIN GPIO_NUM_4 -#define LIMIT_MASK B11011 // OK to comment out to use pin for other features #define STEPPERS_DISABLE_PIN GPIO_NUM_13 @@ -77,26 +76,26 @@ #ifdef HOMING_CYCLE_0 // undefine from config.h #undef HOMING_CYCLE_0 #endif -//#define HOMING_CYCLE_0 (1<. +*/ +#define MACHINE_NAME "ESP32 I2S XXYYZZ Axis Driver Board (StepStick)" + +#ifdef N_AXIS + #undef N_AXIS +#endif +#define N_AXIS 3 + +#ifdef ENABLE_SD_CARD + #undef ENABLE_SD_CARD +#endif + +// === Special Features + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +// Define USE_I2S_OUT_STREAM if buffering is used. +// (there will be a delay between the specified I/O operation and the actual I/O execution) +#define USE_I2S_OUT_STREAM +#undef USE_RMT_STEPS + +#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set + +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 + + +#define STEPPER_MS1 GPIO_NUM_23 // MOSI +#define STEPPER_MS2 GPIO_NUM_18 // SCK + +#define STEPPER_X_MS3 I2SO(3) // X_CS +#define STEPPER_Y_MS3 I2SO(6) // Y_CS +#define STEPPER_Z_MS3 I2SO(11) // Z_CS +#define STEPPER_A_MS3 I2SO(14) // A_CS +#define STEPPER_B_MS3 I2SO(19) // B_CS +#define STEPPER_C_MS3 I2SO(22) // C_CS + +#define STEPPER_RESET GPIO_NUM_19 + +#define USE_GANGED_AXES // allow two motors on an axis + +#define X_DISABLE_PIN I2SO(0) +#define X_DIRECTION_PIN I2SO(1) +#define X_STEP_PIN I2SO(2) + +#define X2_DIRECTION_PIN I2SO(4) +#define X2_STEP_PIN I2SO(5) +#define X2_DISABLE_PIN I2SO(7) + +#define Y_DISABLE_PIN I2SO(8) +#define Y_DIRECTION_PIN I2SO(9) +#define Y_STEP_PIN I2SO(10) + +#define Y2_DIRECTION_PIN I2SO(12) +#define Y2_STEP_PIN I2SO(13) +#define Y2_DISABLE_PIN I2SO(15) + +#define Z_DISABLE_PIN I2SO(16) +#define Z_DIRECTION_PIN I2SO(17) +#define Z_STEP_PIN I2SO(18) + +#define Z2_DIRECTION_PIN I2SO(20) +#define Z2_STEP_PIN I2SO(21) +#define Z2_DISABLE_PIN I2SO(23) + + +#define SPINDLE_TYPE SPINDLE_TYPE_PWM // only one spindle at a time +#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 +#define SPINDLE_ENABLE_PIN GPIO_NUM_4 +#define SPINDLE_DIR_PIN GPIO_NUM_16 + +#define X_LIMIT_PIN GPIO_NUM_36 +#define Y_LIMIT_PIN GPIO_NUM_39 +#define Z_LIMIT_PIN GPIO_NUM_34 +//#define A_LIMIT_PIN GPIO_NUM_35 +//#define B_LIMIT_PIN GPIO_NUM_32 +//#define C_LIMIT_PIN GPIO_NUM_33 + +#define PROBE_PIN GPIO_NUM_25 + +#define COOLANT_MIST_PIN GPIO_NUM_2 + + + +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/Machines/i2s_out_xyzabc.h b/Grbl_Esp32/Machines/i2s_out_xyzabc.h new file mode 100644 index 00000000..f2134a23 --- /dev/null +++ b/Grbl_Esp32/Machines/i2s_out_xyzabc.h @@ -0,0 +1,104 @@ +/* + i2s_out_xyzabc.h + Part of Grbl_ESP32 + Pin assignments for the ESP32 I2S 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 "ESP32 I2S 6 Axis Driver Board (StepStick)" + +#ifdef N_AXIS + #undef N_AXIS +#endif +#define N_AXIS 6 + +#ifdef ENABLE_SD_CARD + #undef ENABLE_SD_CARD +#endif + +// === Special Features + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +// Define USE_I2S_OUT_STREAM if buffering is used. +// (there will be a delay between the specified I/O operation and the actual I/O execution) +#define USE_I2S_OUT_STREAM +#undef USE_RMT_STEPS + +#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set + +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 + + +#define STEPPER_MS1 GPIO_NUM_23 // MOSI +#define STEPPER_MS2 GPIO_NUM_18 // SCK + +#define STEPPER_X_MS3 I2SO(3) // X_CS +#define STEPPER_Y_MS3 I2SO(6) // Y_CS +#define STEPPER_Z_MS3 I2SO(11) // Z_CS +#define STEPPER_A_MS3 I2SO(14) // A_CS +#define STEPPER_B_MS3 I2SO(19) // B_CS +#define STEPPER_C_MS3 I2SO(22) // C_CS + +#define STEPPER_RESET GPIO_NUM_19 + +#define X_DISABLE_PIN I2SO(0) +#define X_DIRECTION_PIN I2SO(1) +#define X_STEP_PIN I2SO(2) + +#define Y_DIRECTION_PIN I2SO(4) +#define Y_STEP_PIN I2SO(5) +#define Y_DISABLE_PIN I2SO(7) + +#define Z_DISABLE_PIN I2SO(8) +#define Z_DIRECTION_PIN I2SO(9) +#define Z_STEP_PIN I2SO(10) + +#define A_DIRECTION_PIN I2SO(12) +#define A_STEP_PIN I2SO(13) +#define A_DISABLE_PIN I2SO(15) + +#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 C_DIRECTION_PIN I2SO(20) +#define C_STEP_PIN I2SO(21) +//#define C_CS_PIN I2SO(22) +#define C_DISABLE_PIN I2SO(23) + + +#define SPINDLE_TYPE SPINDLE_TYPE_PWM // only one spindle at a time +#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 +#define SPINDLE_ENABLE_PIN GPIO_NUM_4 +#define SPINDLE_DIR_PIN GPIO_NUM_16 + +#define X_LIMIT_PIN GPIO_NUM_36 +#define Y_LIMIT_PIN GPIO_NUM_39 +#define Z_LIMIT_PIN GPIO_NUM_34 +#define A_LIMIT_PIN GPIO_NUM_35 +#define B_LIMIT_PIN GPIO_NUM_32 +#define C_LIMIT_PIN GPIO_NUM_33 + +#define PROBE_PIN GPIO_NUM_25 + +#define COOLANT_MIST_PIN GPIO_NUM_2 + + + +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/Machines/i2s_out_xyzabc_trinamic.h b/Grbl_Esp32/Machines/i2s_out_xyzabc_trinamic.h new file mode 100644 index 00000000..ac08274e --- /dev/null +++ b/Grbl_Esp32/Machines/i2s_out_xyzabc_trinamic.h @@ -0,0 +1,105 @@ +/* + i2s_out_xyzabc_trinamic.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 "ESP32 SPI 6 Axis Driver Board Trinamic" + +#ifdef N_AXIS + #undef N_AXIS +#endif +#define N_AXIS 6 + +// === Special Features + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +// Define USE_I2S_OUT_STREAM if buffering is used. +// (there will be a delay between the specified I/O operation and the actual I/O execution) +#define USE_I2S_OUT_STREAM +#undef USE_RMT_STEPS + +#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 2130 +#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 2130 +#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 2130 +#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 2130 +#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 2130 +#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 + +/* +#define SPINDLE_TYPE SPINDLE_TYPE_PWM // only one spindle at a time +#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 +#define SPINDLE_ENABLE_PIN GPIO_NUM_4 +#define SPINDLE_DIR_PIN GPIO_NUM_16 +*/ +#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 +#define B_LIMIT_PIN GPIO_NUM_39 +#define C_LIMIT_PIN GPIO_NUM_36 + +#define SPINDLE_TYPE SPINDLE_TYPE_RELAY +#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 + +#define PROBE_PIN GPIO_NUM_25 + +#define COOLANT_MIST_PIN GPIO_NUM_2 + +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/Machines/lowrider_v1p2.h b/Grbl_Esp32/Machines/lowrider_v1p2.h index 4556c9cf..203f345c 100644 --- a/Grbl_Esp32/Machines/lowrider_v1p2.h +++ b/Grbl_Esp32/Machines/lowrider_v1p2.h @@ -1,25 +1,20 @@ /* - lowrider.h + lowrider_v1p2.h Part of Grbl_ESP32 - Pin assignments for the Buildlog.net MPCNC controller used in lowrider mode. Low rider has (2) Y and Z and one X motor These will not match the silkscreen or schematic descriptions...see definitions below https://github.com/bdring/Grbl_ESP32_MPCNC_Controller - 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 . */ @@ -33,15 +28,18 @@ #define Y_STEP_PIN GPIO_NUM_27 // use Z labeled connector #define Y_DIRECTION_PIN GPIO_NUM_33 // use Z labeled connector -#define Z_STEP_PIN GPIO_NUM_14 // use Y labeled connector -#define Z2_STEP_PIN GPIO_NUM_21 // ganged motor -#define Z_DIRECTION_PIN GPIO_NUM_25 // use Y labeled connector +#define Z_STEP_PIN GPIO_NUM_12 // use X labeled connector +#define Z2_STEP_PIN GPIO_NUM_22 // use X labeled connector +#define Z_DIRECTION_PIN GPIO_NUM_26 // use X labeled connector +#define Z2_DIRECTION_PIN Z_DIRECTION_PIN #define Z_AXIS_SQUARING #define X_STEP_PIN GPIO_NUM_12 // use X labeled connector #define X2_STEP_PIN GPIO_NUM_22 // ganged motor #define X_DIRECTION_PIN GPIO_NUM_26 // use X labeled connector -#define X_AXIS_SQUARING +#define X2_DIRECTION_PIN X_DIRECTION_PIN + +#define X_AXIS_SQUARING // OK to comment out to use pin for other features #define STEPPERS_DISABLE_PIN GPIO_NUM_13 @@ -66,8 +64,6 @@ #define Z_LIMIT_PIN GPIO_NUM_4 #define X_LIMIT_PIN GPIO_NUM_17 -#define LIMIT_MASK B111 - #ifndef ENABLE_SOFTWARE_DEBOUNCE // V1P2 does not have R/C filters #define ENABLE_SOFTWARE_DEBOUNCE #endif diff --git a/Grbl_Esp32/Machines/midtbot.h b/Grbl_Esp32/Machines/midtbot.h index e0c73329..4e93d823 100644 --- a/Grbl_Esp32/Machines/midtbot.h +++ b/Grbl_Esp32/Machines/midtbot.h @@ -22,7 +22,7 @@ along with Grbl_ESP32. If not, see . */ -#define MACHINE_NAME "midTbot" +#define MACHINE_NAME "MIDTBOT" #define SPINDLE_TYPE SPINDLE_TYPE_NONE @@ -40,29 +40,23 @@ #define X_LIMIT_PIN GPIO_NUM_2 #define Y_LIMIT_PIN GPIO_NUM_4 -#define LIMIT_MASK B11 -#define USE_SERVO_AXES - -#define SERVO_Z_PIN GPIO_NUM_27 -#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 Z_SERVO_PIN GPIO_NUM_27 +#define Z_SERVO_RANGE_MIN 0.0 +#define Z_SERVO_RANGE_MAX 5.0 // redefine some stuff from config.h #ifdef HOMING_CYCLE_0 #undef HOMING_CYCLE_0 #endif -#define HOMING_CYCLE_0 (1<. */ -#define MACHINE_NAME "Polar Coaster" +#define MACHINE_NAME "POLAR_COASTER" // This causes the custom code file to be included in the build // via ../custom_code.cpp @@ -45,17 +45,11 @@ #define STEPPERS_DISABLE_PIN GPIO_NUM_17 -#define USE_SERVO_AXES - -#define SERVO_Z_PIN GPIO_NUM_16 -#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 Z_SERVO_PIN GPIO_NUM_16 +#define Z_SERVO_RANGE_MIN 0.0 +#define Z_SERVO_RANGE_MAX 5.0 #define X_LIMIT_PIN GPIO_NUM_4 -#define LIMIT_MASK B1 #define SPINDLE_TYPE SPINDLE_TYPE_NONE @@ -81,7 +75,7 @@ #ifdef HOMING_CYCLE_0 #undef HOMING_CYCLE_0 #endif -#define HOMING_CYCLE_0 (1<. -*/ - -#define MACHINE_NAME "SERVO_AXIS" - -// Pick a board version -//#define PEN_LASER_V1 -#define PEN_LASER_V2 - -#define X_STEP_PIN GPIO_NUM_12 -#define X_DIRECTION_PIN GPIO_NUM_26 - -#define STEPPERS_DISABLE_PIN GPIO_NUM_13 - -#ifdef PEN_LASER_V1 - #define X_LIMIT_PIN GPIO_NUM_2 -#endif - -#ifdef PEN_LASER_V2 - #define X_LIMIT_PIN GPIO_NUM_15 -#endif - -#define Y_LIMIT_PIN GPIO_NUM_4 -#define LIMIT_MASK B11 - -// If SPINDLE_OUTPUT_PIN is commented out, this frees up the pin, but Grbl will still -// use a virtual spindle. Do not comment out the other parameters for the spindle. -#define SPINDLE_OUTPUT_PIN GPIO_NUM_17 // Laser PWM -// PWM Generator is based on 80,000,000 Hz counter -// Therefor the freq determines the resolution -// 80,000,000 / freq = max resolution -// For 5000 that is 80,000,000 / 5000 = 16000 -// round down to nearest bit count for SPINDLE_PWM_MAX_VALUE -//#define SPINDLE_PWM_BASE_FREQ 5000 // Hz -#define SPINDLE_PWM_OFF_VALUE 0 - -#ifndef SPINDLE_PWM_MIN_VALUE - #define SPINDLE_PWM_MIN_VALUE 1 // Must be greater than zero. -#endif - -#define SERVO_Y_PIN GPIO_NUM_14 -#define SERVO_Y_RANGE_MIN 0.0 -#define SERVO_Y_RANGE_MAX 30.0 - -#define SERVO_Z_PIN GPIO_NUM_27 -#define SERVO_Z_RANGE_MIN 0.0 -#define SERVO_Z_RANGE_MAX 20.0 - -// defaults -#define DEFAULT_STEP_PULSE_MICROSECONDS 3 -#define DEFAULT_STEPPER_IDLE_LOCK_TIME 250 // stay on - -#define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t -#define DEFAULT_DIRECTION_INVERT_MASK 0 // 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 0 -#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_SPINDLE_RPM_MAX 1000.0 // rpm -#define DEFAULT_SPINDLE_RPM_MIN 0.0 // rpm - -#define DEFAULT_LASER_MODE 0 // false - -#define DEFAULT_X_STEPS_PER_MM 40 // half turn on a stepper -#define DEFAULT_Y_STEPS_PER_MM 100.0 // default calibration value -#define DEFAULT_Z_STEPS_PER_MM 100.0 // default calibration value - -#define DEFAULT_X_MAX_RATE 2000.0 // mm/min -#define DEFAULT_Y_MAX_RATE 2000.0 // mm/min -#define DEFAULT_Z_MAX_RATE 2000.0 // mm/min - -#define DEFAULT_X_ACCELERATION (50.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 -#define DEFAULT_Y_ACCELERATION (50.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 -#define DEFAULT_Z_ACCELERATION (50.0*60*60) - -#define DEFAULT_X_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value. -#define DEFAULT_Y_MAX_TRAVEL 100.0 // default calibration value -#define DEFAULT_Z_MAX_TRAVEL 100.0 // default calibration value diff --git a/Grbl_Esp32/Machines/spi_daisy_4axis_xyyz.h b/Grbl_Esp32/Machines/spi_daisy_4axis_xyyz.h index 917a3877..1c64538e 100644 --- a/Grbl_Esp32/Machines/spi_daisy_4axis_xyyz.h +++ b/Grbl_Esp32/Machines/spi_daisy_4axis_xyyz.h @@ -1,5 +1,5 @@ /* - spi_daisy_4axisxyyz.h + spi_daisy_4axis_xyyz.h Part of Grbl_ESP32 Pin assignments for a 3-axis with Y ganged using Triaminic drivers @@ -23,16 +23,18 @@ along with Grbl_ESP32. If not, see . */ -#define MACHINE_NAME "SPI Daisy 4x XYYZ" +#define MACHINE_NAME "SPI_DAISY_4X_xyyz" #ifdef N_AXIS #undef N_AXIS #endif #define N_AXIS 3 // can be 3 or 4. (if 3 install bypass jumper next to the A driver) -#define USE_TRINAMIC #define TRINAMIC_DAISY_CHAIN +#define TRINAMIC_RUN_MODE TRINAMIC_MODE_COOLSTEP +#define TRINAMIC_HOMING_MODE TRINAMIC_MODE_COOLSTEP + // Use SPI enable instead of the enable pin // The hardware enable pin is tied to ground #define USE_TRINAMIC_ENABLE @@ -41,37 +43,33 @@ #define USE_GANGED_AXES // Y motor connects to the 1st driver -#define X_DRIVER_TMC2130 // Which Driver Type? -#define X_RSENSE TMC2130_RSENSE_DEFAULT +#define X_TRINAMIC_DRIVER 2130 // Which Driver Type? +#define X_RSENSE TMC2130_RSENSE_DEFAULT #define X_STEP_PIN GPIO_NUM_12 #define X_DIRECTION_PIN GPIO_NUM_14 -#define X_TRINAMIC // using SPI control #define X_CS_PIN GPIO_NUM_17 // Daisy Chain, all share same CS pin // Y motor connects to the 2nd driver -#define Y_DRIVER_TMC2130 // Which Driver Type? +#define Y_TRINAMIC_DRIVER 2130 // Which Driver Type? #define Y_RSENSE TMC2130_RSENSE_DEFAULT #define Y_STEP_PIN GPIO_NUM_27 #define Y_DIRECTION_PIN GPIO_NUM_26 -#define Y_TRINAMIC // using SPI control #define Y_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin // Y2 motor connects to the 2nd driver -#define Y2_DRIVER_TMC2130 // Which Driver Type? -#define Y2_RSENSE TMC2130_RSENSE_DEFAULT +#define Y2_TRINAMIC_DRIVER 2130 // Which Driver Type? +#define Y2_RSENSE TMC2130_RSENSE_DEFAULT #define Y2_STEP_PIN GPIO_NUM_15 // Z on schem #define Y2_DIRECTION_PIN GPIO_NUM_2 // Z on schem -#define Y2_TRINAMIC // using SPI control #define Y2_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin #define Y_AXIS_SQUARING // optional // Z Axis motor connects to the 4th driver -#define Z_DRIVER_TMC2130 // Which Driver Type? +#define Z_TRINAMIC_DRIVER 2130 // Which Driver Type? #define Z_RSENSE TMC2130_RSENSE_DEFAULT #define Z_STEP_PIN GPIO_NUM_33 // A on schem #define Z_DIRECTION_PIN GPIO_NUM_32 // A on schem -#define Z_TRINAMIC // using SPI control #define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin // Mist is a 3.3V output @@ -95,5 +93,3 @@ #define Y_LIMIT_PIN GPIO_NUM_39 #define Z_LIMIT_PIN GPIO_NUM_34 -#define LIMIT_MASK B0111 - diff --git a/Grbl_Esp32/Machines/spi_daisy_4axis_xyz.h b/Grbl_Esp32/Machines/spi_daisy_4axis_xyz.h index 1187bd0c..cdcac161 100644 --- a/Grbl_Esp32/Machines/spi_daisy_4axis_xyz.h +++ b/Grbl_Esp32/Machines/spi_daisy_4axis_xyz.h @@ -1,5 +1,5 @@ /* - spi_daisy_4axis.h + spi_daisy_4axis_xyz.h Part of Grbl_ESP32 Pin assignments for a 4-axis machine using Triaminic drivers @@ -23,39 +23,38 @@ along with Grbl_ESP32. If not, see . */ -#define MACHINE_NAME "SPI Daisy 4x XYZ" +#define MACHINE_NAME "SPI_DAISY_4X_XYZ" #ifdef N_AXIS #undef N_AXIS #endif #define N_AXIS 3 -#define USE_TRINAMIC #define TRINAMIC_DAISY_CHAIN +#define TRINAMIC_RUN_MODE TRINAMIC_MODE_COOLSTEP +#define TRINAMIC_HOMING_MODE TRINAMIC_MODE_COOLSTEP + // Use SPI enable instead of the enable pin // The hardware enable pin is tied to ground #define USE_TRINAMIC_ENABLE -#define X_DRIVER_TMC2130 // Which Driver Type? +#define X_TRINAMIC_DRIVER 2130 // Which Driver Type? #define X_RSENSE TMC2130_RSENSE_DEFAULT #define X_STEP_PIN GPIO_NUM_12 #define X_DIRECTION_PIN GPIO_NUM_14 -#define X_TRINAMIC // using SPI control #define X_CS_PIN GPIO_NUM_17 // Daisy Chain, all share same CS pin -#define Y_DRIVER_TMC2130 // Which Driver Type? +#define Y_TRINAMIC_DRIVER 2130 // Which Driver Type? #define Y_RSENSE TMC2130_RSENSE_DEFAULT #define Y_STEP_PIN GPIO_NUM_27 #define Y_DIRECTION_PIN GPIO_NUM_26 -#define Y_TRINAMIC // using SPI control #define Y_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin -#define Z_DRIVER_TMC2130 // Which Driver Type? +#define Z_TRINAMIC_DRIVER 2130 // Which Driver Type? #define Z_RSENSE TMC2130_RSENSE_DEFAULT #define Z_STEP_PIN GPIO_NUM_15 #define Z_DIRECTION_PIN GPIO_NUM_2 -#define Z_TRINAMIC // using SPI control #define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin @@ -79,5 +78,4 @@ #define X_LIMIT_PIN GPIO_NUM_36 #define Y_LIMIT_PIN GPIO_NUM_39 #define Z_LIMIT_PIN GPIO_NUM_34 -#define LIMIT_MASK B0111 diff --git a/Grbl_Esp32/Machines/spi_daisy_4axis_xyza.h b/Grbl_Esp32/Machines/spi_daisy_4axis_xyza.h index 93a03844..da20a4ab 100644 --- a/Grbl_Esp32/Machines/spi_daisy_4axis_xyza.h +++ b/Grbl_Esp32/Machines/spi_daisy_4axis_xyza.h @@ -1,5 +1,5 @@ /* - spi_daisy_4axis.h + spi_daisy_4axis_xyza.h Part of Grbl_ESP32 Pin assignments for a 4-axis machine using Triaminic drivers @@ -23,54 +23,51 @@ along with Grbl_ESP32. If not, see . */ -#define MACHINE_NAME "SPI Daisy 4x XYZA" +#define MACHINE_NAME "SPI_DAISY_4X XYZA" #ifdef N_AXIS #undef N_AXIS #endif #define N_AXIS 4 // can be 3 or 4. (if 3 install bypass jumper next to the A driver) -#define USE_TRINAMIC #define TRINAMIC_DAISY_CHAIN +#define TRINAMIC_RUN_MODE TRINAMIC_MODE_STEALTHCHOP + // Use SPI enable instead of the enable pin // The hardware enable pin is tied to ground #define USE_TRINAMIC_ENABLE -#define X_DRIVER_TMC2130 // Which Driver Type? +#define X_TRINAMIC_DRIVER 2130 // Which Driver Type? #define X_RSENSE TMC2130_RSENSE_DEFAULT #define X_STEP_PIN GPIO_NUM_12 #define X_DIRECTION_PIN GPIO_NUM_14 -#define X_TRINAMIC // using SPI control #define X_CS_PIN GPIO_NUM_17 // Daisy Chain, all share same CS pin -#define Y_DRIVER_TMC2130 // Which Driver Type? +#define Y_TRINAMIC_DRIVER 2130 // Which Driver Type? #define Y_RSENSE TMC2130_RSENSE_DEFAULT #define Y_STEP_PIN GPIO_NUM_27 #define Y_DIRECTION_PIN GPIO_NUM_26 -#define Y_TRINAMIC // using SPI control #define Y_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin -#define Z_DRIVER_TMC2130 // Which Driver Type? +#define Z_TRINAMIC_DRIVER 2130 // Which Driver Type? #define Z_RSENSE TMC2130_RSENSE_DEFAULT #define Z_STEP_PIN GPIO_NUM_15 #define Z_DIRECTION_PIN GPIO_NUM_2 -#define Z_TRINAMIC // using SPI control #define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin -#define A_DRIVER_TMC2130 // Which Driver Type? -#define A_RSENSE TMC2130_RSENSE_DEFAULT -#define A_STEP_PIN GPIO_NUM_33 -#define A_DIRECTION_PIN GPIO_NUM_32 -#define A_TRINAMIC // using SPI control -#define A_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin +#define A_TRINAMIC_DRIVER 2130 // Which Driver Type? +#define A_RSENSE TMC2130_RSENSE_DEFAULT +#define A_STEP_PIN GPIO_NUM_33 +#define A_DIRECTION_PIN GPIO_NUM_32 +#define A_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin // Mist is a 3.3V output // Turn on with M7 and off with M9 #define COOLANT_MIST_PIN GPIO_NUM_21 -#define SPINDLE_OUTPUT_PIN GPIO_NUM_25 +#define SPINDLE_OUTPUT_PIN GPIO_NUM_25 #define SPINDLE_ENABLE_PIN GPIO_NUM_4 // Relay operation @@ -90,5 +87,4 @@ #define X_LIMIT_PIN GPIO_NUM_36 #define Y_LIMIT_PIN GPIO_NUM_39 #define Z_LIMIT_PIN GPIO_NUM_34 -#define A_LIMIT_PIN GPIO_NUM_35 -#define LIMIT_MASK B1111 +#define A_LIMIT_PIN GPIO_NUM_35 diff --git a/Grbl_Esp32/Machines/spindle_class_test.h b/Grbl_Esp32/Machines/spindle_class_test.h index 1a7d6654..1a675779 100644 --- a/Grbl_Esp32/Machines/spindle_class_test.h +++ b/Grbl_Esp32/Machines/spindle_class_test.h @@ -32,7 +32,6 @@ #define Z_STEP_PIN GPIO_NUM_27 #define Z_DIRECTION_PIN GPIO_NUM_33 -#define LIMIT_MASK B111 #define X_LIMIT_PIN GPIO_NUM_17 #define Y_LIMIT_PIN GPIO_NUM_4 #define Z_LIMIT_PIN GPIO_NUM_16 @@ -48,4 +47,4 @@ /* #define SPINDLE_TYPE SPINDLE_TYPE_BESC #define SPINDLE_OUTPUT_PIN GPIO_NUM_27 // -*/ \ No newline at end of file +*/ diff --git a/Grbl_Esp32/Machines/template.h b/Grbl_Esp32/Machines/template.h index 9c4272c6..6fa66902 100644 --- a/Grbl_Esp32/Machines/template.h +++ b/Grbl_Esp32/Machines/template.h @@ -84,11 +84,6 @@ // #define Z_STEP_PIN GPIO_NUM_27 // #define Z_DIRECTION_PIN GPIO_NUM_33 -// The 1 bits in LIMIT_MASK set the axes that have limit switches -// For example, if the Y axis has no limit switches but the -// X, Z, A and B axes do, the LIMIT_MASK value would be B11101 -// #define LIMIT_MASK B111 - // #define X_LIMIT_PIN GPIO_NUM_17 // #define Y_LIMIT_PIN GPIO_NUM_4 // #define Z_LIMIT_PIN GPIO_NUM_16 @@ -151,10 +146,10 @@ // not at all. // #undef HOMING_CYCLE_0 -// #define HOMING_CYCLE_0 ((1<. + TODO + Make sure public/private/protected is cleaned up. + Only a few Unipolar axes have been setup in init() + Get rid of Z_SERVO, just reply on Z_SERVO_PIN + Deal with custom machine ... machine_trinamic_setup(); + Class is ready to deal with non SPI pins, but they have not been needed yet. + It would be nice in the config message though + Testing + Done (success) + 3 Axis (3 Standard Steppers) + MPCNC (ganged with shared direction pin) + TMC2130 Pen Laser (trinamics, stallguard tuning) + Unipolar + TODO + 4 Axis SPI (Daisy Chain, Ganged with unique direction pins) + Reference + TMC2130 Datasheet https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC2130_datasheet.pdf +*/ + +#include "../grbl.h" +#include "TrinamicDriverClass.cpp" +#include "StandardStepperClass.cpp" +#include "UnipolarMotorClass.cpp" +#include "RcServoClass.cpp" +//#include "SolenoidClass.cpp" + +Motor* myMotor[MAX_AXES][MAX_GANGED]; // number of axes (normal and ganged) +static TaskHandle_t readSgTaskHandle = 0; // for realtime stallguard data diaplay +static TaskHandle_t servoUpdateTaskHandle = 0; + +uint8_t rmt_chan_num[MAX_AXES][MAX_GANGED]; +rmt_item32_t rmtItem[2]; +rmt_config_t rmtConfig; + +bool motor_class_steps; // true if at least one motor class is handling steps + +void init_motors() { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Init Motors"); + +#ifdef X_TRINAMIC_DRIVER + myMotor[X_AXIS][0] = new TrinamicDriver(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE, get_next_trinamic_driver_index()); +#elif defined(X_SERVO_PIN) + myMotor[X_AXIS][0] = new RcServo(X_AXIS, X_SERVO_PIN, X_SERVO_RANGE_MIN, X_SERVO_RANGE_MAX); +#elif defined(X_UNIPOLAR) + myMotor[X_AXIS][0] = new UnipolarMotor(X_AXIS, X_PIN_PHASE_0, X_PIN_PHASE_1, X_PIN_PHASE_2, X_PIN_PHASE_3); +#elif defined(X_STEP_PIN) + myMotor[X_AXIS][0] = new StandardStepper(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN); +#else + myMotor[X_AXIS][0] = new Nullmotor(); +#endif + +#ifdef X2_TRINAMIC_DRIVER + myMotor[X_AXIS][1] = new TrinamicDriver(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE, get_next_trinamic_driver_index()); +#elif defined(X2_SERVO_PIN) + myMotor[X_AXIS][1] = new RcServo(X2_AXIS, X2_SERVO_PIN, X2_SERVO_RANGE_MIN, X2_SERVO_RANGE_MAX); +#elif defined(X2_UNIPOLAR) + myMotor[X_AXIS][1] = new UnipolarMotor(X2_AXIS, X2_PIN_PHASE_0, X2_PIN_PHASE_1, X2_PIN_PHASE_2, X2_PIN_PHASE_3); +#elif defined(X2_STEP_PIN) + myMotor[X_AXIS][1] = new StandardStepper(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN); +#else + myMotor[X_AXIS][1] = new Nullmotor(); +#endif + + + // this WILL be done better with settings +#ifdef Y_TRINAMIC_DRIVER + myMotor[Y_AXIS][0] = new TrinamicDriver(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE, get_next_trinamic_driver_index()); +#elif defined(Y_SERVO_PIN) + myMotor[Y_AXIS][0] = new RcServo(Y_AXIS, Y_SERVO_PIN, Y_SERVO_RANGE_MIN, Y_SERVO_RANGE_MAX); +#elif defined(Y_UNIPOLAR) + myMotor[Y_AXIS][0] = new UnipolarMotor(Y_AXIS, Y_PIN_PHASE_0, Y_PIN_PHASE_1, Y_PIN_PHASE_2, Y_PIN_PHASE_3); +#elif defined(Y_STEP_PIN) + myMotor[Y_AXIS][0] = new StandardStepper(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN); +#else + myMotor[Y_AXIS][0] = new Nullmotor(); +#endif + +#ifdef Y2_TRINAMIC_DRIVER + myMotor[Y_AXIS][1] = new TrinamicDriver(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE, get_next_trinamic_driver_index()); +#elif defined(Y2_SERVO_PIN) + myMotor[Y_AXIS][1] = new RcServo(Y2_AXIS, Y2_SERVO_PIN, Y2_SERVO_RANGE_MIN, Y2_SERVO_RANGE_MAX); +#elif defined(Y2_UNIPOLAR) + myMotor[Y_AXIS][1] = new UnipolarMotor(Y2_AXIS, Y2_PIN_PHASE_0, Y2_PIN_PHASE_1, Y2_PIN_PHASE_2, Y2_PIN_PHASE_3); +#elif defined(Y2_STEP_PIN) + myMotor[Y_AXIS][1] = new StandardStepper(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN); +#else + myMotor[Y_AXIS][1] = new Nullmotor(); +#endif + + + // this WILL be done better with settings +#ifdef Z_TRINAMIC_DRIVER + myMotor[Z_AXIS][0] = new TrinamicDriver(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE, get_next_trinamic_driver_index()); +#elif defined(Z_SERVO_PIN) + myMotor[Z_AXIS][0] = new RcServo(Z_AXIS, Z_SERVO_PIN, Z_SERVO_RANGE_MIN, Z_SERVO_RANGE_MAX); +#elif defined(Z_UNIPOLAR) + myMotor[Z_AXIS][0] = new UnipolarMotor(Z_AXIS, Z_PIN_PHASE_0, Z_PIN_PHASE_1, Z_PIN_PHASE_2, Z_PIN_PHASE_3); +#elif defined(Z_STEP_PIN) + myMotor[Z_AXIS][0] = new StandardStepper(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN); +#else + myMotor[Z_AXIS][0] = new Nullmotor(); +#endif + +#ifdef Z2_TRINAMIC_DRIVER + myMotor[Z_AXIS][1] = new TrinamicDriver(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE, get_next_trinamic_driver_index()); +#elif defined(Z2_SERVO_PIN) + myMotor[Z_AXIS][1] = new RcServo(Z2_AXIS, Z2_SERVO_PIN, Z2_SERVO_RANGE_MIN, Z2_SERVO_RANGE_MAX); +#elif defined(Z2_UNIPOLAR) + myMotor[Z_AXIS][1] = new UnipolarMotor(Z2_AXIS, Z2_PIN_PHASE_0, Z2_PIN_PHASE_1, Z2_PIN_PHASE_2, Z2_PIN_PHASE_3); +#elif defined(Z2_STEP_PIN) + myMotor[Z_AXIS][1] = new StandardStepper(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN); +#else + myMotor[Z_AXIS][1] = new Nullmotor(); +#endif + + // this WILL be done better with settings +#ifdef A_TRINAMIC_DRIVER + myMotor[A_AXIS][0] = new TrinamicDriver(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE, get_next_trinamic_driver_index()); +#elif defined(A_SERVO_PIN) + myMotor[A_AXIS][0] = new RcServo(A_AXIS, A_SERVO_PIN, A_SERVO_RANGE_MIN, A_SERVO_RANGE_MAX); +#elif defined(A_UNIPOLAR) + myMotor[A_AXIS][0] = new UnipolarMotor(A_AXIS, A_PIN_PHASE_0, A_PIN_PHASE_1, A_PIN_PHASE_2, A_PIN_PHASE_3); +#elif defined(A_STEP_PIN) + myMotor[A_AXIS][0] = new StandardStepper(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN); +#else + myMotor[A_AXIS][0] = new Nullmotor(); +#endif + +#ifdef A2_TRINAMIC_DRIVER + myMotor[A_AXIS][1] = new TrinamicDriver(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE, get_next_trinamic_driver_index()); +#elif defined(A2_SERVO_PIN) + myMotor[A_AXIS][1] = new RcServo(A2_AXIS, A2_SERVO_PIN, A2_SERVO_RANGE_MIN, A2_SERVO_RANGE_MAX); +#elif defined(A2_UNIPOLAR) + myMotor[A_AXIS][1] = new UnipolarMotor(A2_AXIS, A2_PIN_PHASE_0, A2_PIN_PHASE_1, A2_PIN_PHASE_2, A2_PIN_PHASE_3); +#elif defined(A2_STEP_PIN) + myMotor[A_AXIS][1] = new StandardStepper(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN); +#else + myMotor[A_AXIS][1] = new Nullmotor(); +#endif + + // this WILL be done better with settings +#ifdef B_TRINAMIC_DRIVER + myMotor[B_AXIS][0] = new TrinamicDriver(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE, get_next_trinamic_driver_index()); +#elif defined(B_SERVO_PIN) + myMotor[B_AXIS][0] = new RcServo(B_AXIS, B_SERVO_PIN, B_SERVO_RANGE_MIN, B_SERVO_RANGE_MAX); +#elif defined(B_UNIPOLAR) + myMotor[B_AXIS][0] = new UnipolarMotor(B_AXIS, B_PIN_PHASE_0, B_PIN_PHASE_1, B_PIN_PHASE_2, B_PIN_PHASE_3); +#elif defined(B_STEP_PIN) + myMotor[B_AXIS][0] = new StandardStepper(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN); +#else + myMotor[B_AXIS][0] = new Nullmotor(); +#endif + +#ifdef B2_TRINAMIC_DRIVER + myMotor[B_AXIS][1] = new TrinamicDriver(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE, get_next_trinamic_driver_index()); +#elif defined(B2_SERVO_PIN) + myMotor[B_AXIS][1] = new RcServo(B2_AXIS, B2_SERVO_PIN, B2_SERVO_RANGE_MIN, B2_SERVO_RANGE_MAX); +#elif defined(B2_UNIPOLAR) + myMotor[B_AXIS][1] = new UnipolarMotor(B2_AXIS, B2_PIN_PHASE_0, B2_PIN_PHASE_1, B2_PIN_PHASE_2, B2_PIN_PHASE_3); +#elif defined(B2_STEP_PIN) + myMotor[B_AXIS][1] = new StandardStepper(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN); +#else + myMotor[B_AXIS][1] = new Nullmotor(); +#endif + + // this WILL be done better with settings +#ifdef C_TRINAMIC_DRIVER + myMotor[C_AXIS][0] = new TrinamicDriver(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE, get_next_trinamic_driver_index()); +#elif defined(C_SERVO_PIN) + myMotor[C_AXIS][0] = new RcServo(C_AXIS, C_SERVO_PIN, C_SERVO_RANGE_MIN, C_SERVO_RANGE_MAX); +#elif defined(C_UNIPOLAR) + myMotor[C_AXIS][0] = new UnipolarMotor(C_AXIS, C_PIN_PHASE_0, C_PIN_PHASE_1, C_PIN_PHASE_2, C_PIN_PHASE_3); +#elif defined(C_STEP_PIN) + myMotor[C_AXIS][0] = new StandardStepper(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN); +#else + myMotor[C_AXIS][0] = new Nullmotor(); +#endif + +#ifdef C2_TRINAMIC_DRIVER + myMotor[C_AXIS][1] = new TrinamicDriver(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE, get_next_trinamic_driver_index()); +#elif defined(C2_SERVO_PIN) + myMotor[C_AXIS][1] = new RcServo(C2_AXIS, C2_SERVO_PIN, C2_SERVO_RANGE_MIN, C2_SERVO_RANGE_MAX); +#elif defined(C2_UNIPOLAR) + myMotor[C_AXIS][1] = new UnipolarMotor(C2_AXIS, C2_PIN_PHASE_0, C2_PIN_PHASE_1, C2_PIN_PHASE_2, C2_PIN_PHASE_3); +#elif defined(C2_STEP_PIN) + myMotor[C_AXIS][1] = new StandardStepper(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN); +#else + myMotor[C_AXIS][1] = new Nullmotor(); +#endif + + +#ifdef USE_STEPSTICK + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Using StepStick Mode"); +#ifdef STEPPER_MS1 + digitalWrite(STEPPER_MS1, HIGH); + pinMode(STEPPER_MS1, OUTPUT); +#endif +#ifdef STEPPER_MS2 + digitalWrite(STEPPER_MS2, HIGH); + pinMode(STEPPER_MS2, OUTPUT); +#endif +#ifdef STEPPER_X_MS3 + digitalWrite(STEPPER_X_MS3, HIGH); + pinMode(STEPPER_X_MS3, OUTPUT); +#endif +#ifdef STEPPER_Y_MS3 + digitalWrite(STEPPER_Y_MS3, HIGH); + pinMode(STEPPER_Y_MS3, OUTPUT); +#endif +#ifdef STEPPER_Z_MS3 + digitalWrite(STEPPER_Z_MS3, HIGH); + pinMode(STEPPER_Z_MS3, OUTPUT); +#endif +#ifdef STEPPER_A_MS3 + digitalWrite(STEPPER_A_MS3, HIGH); + pinMode(STEPPER_A_MS3, OUTPUT); +#endif +#ifdef STEPPER_B_MS3 + digitalWrite(STEPPER_B_MS3, HIGH); + pinMode(STEPPER_B_MS3, OUTPUT); +#endif +#ifdef STEPPER_C_MS3 + digitalWrite(STEPPER_C_MS3, HIGH); + pinMode(STEPPER_C_MS3, OUTPUT); +#endif +#ifdef STEPPER_RESET + // !RESET pin on steppers (MISO On Schematic) + digitalWrite(STEPPER_RESET, HIGH); + pinMode(STEPPER_RESET, OUTPUT); +#endif + +#endif + + if (STEPPERS_DISABLE_PIN != UNDEFINED_PIN) { + pinMode(STEPPERS_DISABLE_PIN, OUTPUT); // global motor enable pin + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "Global stepper disable pin:%s", + pinName(STEPPERS_DISABLE_PIN)); + } + + // certain motors need features to be turned on. Check them here + for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) { + for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { + + if (myMotor[axis][gang_index]->type_id == UNIPOLAR_MOTOR) + motor_class_steps = true; + + // CS Pins of all TMC motors need to be setup before any can be talked to + // ...so init cannot be called via the constructors. This inits them all. + if (myMotor[axis][gang_index]->type_id == TRINAMIC_SPI_MOTOR) + myMotor[axis][gang_index]->init(); + } + } + + // some motor objects require a step signal + motor_class_steps = motors_have_type_id(UNIPOLAR_MOTOR); + + if (motors_have_type_id(TRINAMIC_SPI_MOTOR)) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "TMCStepper Library Ver. 0x%06x", TMCSTEPPER_VERSION); + xTaskCreatePinnedToCore(readSgTask, // task + "readSgTask", // name for task + 4096, // size of task stack + NULL, // parameters + 1, // priority + &readSgTaskHandle, + 0 // core + ); + if (stallguard_debug_mask->get() != 0) + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Stallguard debug enabled: %d", stallguard_debug_mask->get()); + } + + if (motors_have_type_id(RC_SERVO_MOTOR)) { + xTaskCreatePinnedToCore(servoUpdateTask, // task + "servoUpdateTask", // name for task + 4096, // size of task stack + NULL, // parameters + 1, // priority + &servoUpdateTaskHandle, + 0 // core + ); + } +} + + + +void servoUpdateTask(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xUpdate = SERVO_TIMER_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 + + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo update"); + + for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) { + for (uint8_t gang_index = 0; gang_index < 2; gang_index++) + myMotor[axis][gang_index]->update(); + } + + vTaskDelayUntil(&xLastWakeTime, xUpdate); + } +} + +// do any motors match the type_id +bool motors_have_type_id(motor_class_id_t id) { + for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) { + for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { + if (myMotor[axis][gang_index]->type_id == id) + return true; + } + } + return false; +} + + +void motors_set_disable(bool disable) { + static bool previous_state = false; + + if (previous_state == disable) + return; + + previous_state = disable; + + if (step_enable_invert->get()) { + disable = !disable; // Apply pin invert. + } + + digitalWrite(STEPPERS_DISABLE_PIN, 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++) + myMotor[axis][gang_index]->set_disable(disable); + } +} + +void motors_read_settings() { + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Read Settings"); + for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { + for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) + myMotor[axis][gang_index]->read_settings(); + } +} + +// use this to tell all the motors what the current homing mode is +// They can use this to setup things like Stall +void motors_set_homing_mode(uint8_t homing_mask, bool isHoming) { + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "motors_set_homing_mode(%d)", is_homing); + for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { + for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) + if (bit(axis) & homing_mask) + myMotor[axis][gang_index]->set_homing_mode(homing_mask, isHoming); + } +} + + +void motors_set_direction_pins(uint8_t onMask) { + static uint8_t previous_val = 255; // should never be this value + if (previous_val == onMask) + return; + previous_val = onMask; + + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "motors_set_direction_pins:0x%02X", onMask); + + for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) { + for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) + myMotor[axis][gang_index]->set_direction_pins(onMask); + } +} + +// for testing +#ifndef USE_TRINAMIC +// returns the next spi index. We cannot preassign to axes because ganged (X2 type axes) might +// need to be inserted into the order of axes. +uint8_t get_next_trinamic_driver_index() { +#ifdef TRINAMIC_DAISY_CHAIN + static uint8_t index = 1; // they start at 1 + return index++; +#else + return -1; +#endif +} + +// some motor objects, like unipolar need step signals +void motors_step(uint8_t step_mask, uint8_t dir_mask) { + if (motor_class_steps) { // determined in init_motors if any motors need to handle steps + for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { + for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) + myMotor[axis][gang_index]->step(step_mask, dir_mask); + } + } +} + +/* + This will print StallGuard data that is useful for tuning. +*/ +void readSgTask(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xreadSg = 200; // in ticks (typically ms) + + xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. + while (true) { // don't ever return from this or the task dies + if (motorSettingChanged) { + motors_read_settings(); + motorSettingChanged = false; + } + + if (stallguard_debug_mask->get() != 0) { + if (sys.state == STATE_CYCLE || sys.state == STATE_HOMING || sys.state == STATE_JOG) { + for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) { + if (stallguard_debug_mask->get() & bit(axis)) { + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "SG:%d", stallguard_debug_mask->get()); + for (uint8_t gang_index = 0; gang_index < 2; gang_index++) + myMotor[axis][gang_index]->debug_message(); + } + } + } // sys.state + } // if mask + vTaskDelayUntil(&xLastWakeTime, xreadSg); + } +} + + +#ifdef USE_I2S_OUT +// +// Override default function and insert a short delay +// +void TMC2130Stepper::switchCSpin(bool state) { + digitalWrite(_pinCS, state); + i2s_out_delay(); +} +#endif + +#endif + +// ============================== Class Methods ================================================ + +Motor :: Motor() { + type_id = MOTOR; +} + +void Motor :: init() { + _homing_mask = 0; +} + +void Motor :: config_message() {} +void Motor :: debug_message() {} +void Motor :: read_settings() {} +void Motor :: set_disable(bool disable) {} +void Motor :: set_direction_pins(uint8_t onMask) {} +void Motor :: step(uint8_t step_mask, uint8_t dir_mask) {} +bool Motor :: test() {return true;}; // true = OK +void Motor :: update() {} + +void Motor :: set_axis_name() { + sprintf(_axis_name, "%c%s", report_get_axis_letter(axis_index), dual_axis_index ? "2" : ""); +} + +void Motor :: set_homing_mode(uint8_t homing_mask, bool isHoming) { + _homing_mask = homing_mask; +} diff --git a/Grbl_Esp32/Motors/MotorClass.h b/Grbl_Esp32/Motors/MotorClass.h new file mode 100644 index 00000000..072ebbe6 --- /dev/null +++ b/Grbl_Esp32/Motors/MotorClass.h @@ -0,0 +1,218 @@ +/* + MotorClass.h + Header file for Motor Classes + Here is the hierarchy + Motor + Nullmotor + StandardStepper + TrinamicDriver + Unipolar + RC Servo + + These are for motors coordinated by Grbl_ESP32 + See motorClass.cpp for more details + + Part of Grbl_ESP32 + 2020 - Bart Dring + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef MOTORCLASS_H +#define MOTORCLASS_H + +#include "../grbl.h" +#include // https://github.com/teemuatlut/TMCStepper +#include "TrinamicDriverClass.h" +#include "RcServoClass.h" +//#include "SolenoidClass.h" + +extern uint8_t rmt_chan_num[MAX_AXES][2]; +extern rmt_item32_t rmtItem[2]; +extern rmt_config_t rmtConfig; + +typedef enum { + MOTOR, + NULL_MOTOR, + STANDARD_MOTOR, + TRINAMIC_SPI_MOTOR, + UNIPOLAR_MOTOR, + RC_SERVO_MOTOR, + SOLENOID +} motor_class_id_t; + +// These are used for setup and to talk to the motors as a group. +void init_motors(); +uint8_t get_next_trinamic_driver_index(); +bool motors_have_type_id(motor_class_id_t id); +void readSgTask(void* pvParameters); +void motors_read_settings(); +void motors_set_homing_mode(uint8_t homing_mask, bool isHoming); +void motors_set_disable(bool disable); +void motors_set_direction_pins(uint8_t onMask); +void motors_step(uint8_t step_mask, uint8_t dir_mask); +void servoUpdateTask(void* pvParameters); + +extern bool motor_class_steps; // true if at least one motor class is handling steps + +// ==================== Motor Classes ==================== + +class Motor { + public: + Motor(); + + virtual void init(); // not in constructor because this also gets called when $$ settings change + virtual void config_message(); + virtual void debug_message(); + virtual void read_settings(); + virtual void set_homing_mode(uint8_t homing_mask, bool isHoming); + virtual void set_disable(bool disable); + virtual void set_direction_pins(uint8_t onMask); + virtual void step(uint8_t step_mask, uint8_t dir_mask); // only used on Unipolar right now + virtual bool test(); + virtual void set_axis_name(); + virtual void update(); + + motor_class_id_t type_id; + uint8_t is_active = false; + + protected: + uint8_t axis_index; // X_AXIS, etc + uint8_t dual_axis_index; // 0 = primary 1=ganged + + bool _showError; + bool _use_mpos = true; + uint8_t _homing_mask; + char _axis_name[10];// this the name to use when reporting like "X" or "X2" +}; + +class Nullmotor : public Motor { + +}; + +class StandardStepper : public Motor { + public: + StandardStepper(); + StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin); + + virtual void config_message(); + virtual void init(); + virtual void set_direction_pins(uint8_t onMask); + void init_step_dir_pins(); + virtual void set_disable(bool disable); + uint8_t step_pin; + + protected: + bool _invert_step_pin; + uint8_t dir_pin; + uint8_t disable_pin; +}; + +class TrinamicDriver : public StandardStepper { + public: + TrinamicDriver(uint8_t axis_index, + uint8_t step_pin, + uint8_t dir_pin, + uint8_t disable_pin, + uint8_t cs_pin, + uint16_t driver_part_number, + float r_sense, + int8_t spi_index); + + void config_message(); + void init(); + void set_mode(bool isHoming); + void read_settings(); + void trinamic_test_response(); + void trinamic_stepper_enable(bool enable); + void debug_message(); + void set_homing_mode(uint8_t homing_mask, bool ishoming); + void set_disable(bool disable); + bool test(); + + private: + uint32_t calc_tstep(float speed, float percent); + + TMC2130Stepper* tmcstepper; // all other driver types are subclasses of this one + uint8_t _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; +}; + + +class UnipolarMotor : public Motor { + public: + UnipolarMotor(); + UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3); + void init(); + void config_message(); + void set_disable(bool disable); + void step(uint8_t step_mask, uint8_t dir_mask); // only used on Unipolar right now + + private: + uint8_t _pin_phase0; + uint8_t _pin_phase1; + uint8_t _pin_phase2; + uint8_t _pin_phase3; + uint8_t _current_phase; + bool _half_step; + bool _enabled; +}; + +class RcServo : public Motor { + public: + RcServo(); + RcServo(uint8_t axis_index, uint8_t pwm_pin, float min, float max); + virtual void config_message(); + virtual void init(); + void _write_pwm(uint32_t duty); + virtual void set_disable(bool disable); + virtual void update(); + void read_settings(); + void set_homing_mode(bool is_homing, bool isHoming); + + protected: + void set_location(); + void _get_calibration(); + + uint8_t _pwm_pin; + uint8_t _channel_num; + uint32_t _current_pwm_duty; + bool _disabled; + + float _position_min; + float _position_max; // position in millimeters + float _homing_position; + + float _pwm_pulse_min; + float _pwm_pulse_max; +}; + +class Solenoid : public RcServo { + public: + Solenoid(); + Solenoid(uint8_t axis_index, gpio_num_t pwm_pin, float transition_poiont); + void config_message(); + void set_location(); + void update(); + void init(); + void set_disable(bool disable); + + float _transition_poiont; +}; + +#endif diff --git a/Grbl_Esp32/Motors/RcServoClass.cpp b/Grbl_Esp32/Motors/RcServoClass.cpp new file mode 100644 index 00000000..51253138 --- /dev/null +++ b/Grbl_Esp32/Motors/RcServoClass.cpp @@ -0,0 +1,190 @@ +/* + RcServoServoClass.cpp + + This allows an RcServo to be used like any other motor. Serrvos + do have limitation in travel and speed, so you do need to respect that. + + Part of Grbl_ESP32 + + 2020 - Bart Dring + + Servos have a limited travel, so they map the travel across a range in + the current work coordinatee system. The servo can only travel as far + as the range, but the internal axis value can keep going. + + Range: The range is specified in the machine definition file with... + #define X_SERVO_RANGE_MIN 0.0 + #define X_SERVO_RANGE_MAX 5.0 + + Direction: The direction can be changed using the $3 setting for the axis + + Homing: During homing, the servo will move to one of the endpoints. The + endpoint is determined by the $23 or $HomingDirInvertMask setting for the axis. + Do not define a homing cycle for the axis with the servo. + You do need at least 1 homing cycle. TODO: Fix this + + Calibration. You can tweak the endpoints using the $10n or nStepsPerMm and + $13n or $xMaxTravel setting, where n is the axis. + The value is a percent. If you secify a percent outside the + the range specified by the values below, it will be reset to 100.0 (100% ... no change) + The calibration adjusts in direction of positive momement, so a value above 100% moves + towards the higher axis value. + + #define SERVO_CAL_MIN + #define SERVO_CAL_MAX + + Grbl_ESP32 is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +RcServo :: RcServo() { + +} + +RcServo :: RcServo(uint8_t axis_index, uint8_t pwm_pin, float min, float max) { + type_id = RC_SERVO_MOTOR; + this->axis_index = axis_index % MAX_AXES; + this->dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged + this->_pwm_pin = pwm_pin; + _position_min = min; + _position_max = max; + init(); +} + +void RcServo :: init() { + read_settings(); + _channel_num = sys_get_next_PWM_chan_num(); + ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS); + ledcAttachPin(_pwm_pin, _channel_num); + _current_pwm_duty = 0; + is_active = true; // as opposed to NullMotors, this is a real motor + set_axis_name(); + config_message(); +} + +void RcServo :: config_message() { + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "%s Axis RC Servo motor Output:%d Min:%5.3fmm Max:%5.3fmm", + _axis_name, + _pwm_pin, + _position_min, + _position_max); +} + +void RcServo::_write_pwm(uint32_t duty) { + // to prevent excessive calls to ledcWrite, make sure duty hass changed + if (duty == _current_pwm_duty) + return; + + _current_pwm_duty = duty; + + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Servo Pwm %d", _axis_name, duty); + ledcWrite(_channel_num, duty); +} + +// sets the PWM to zero. This allows most servos to be manually moved +void RcServo::set_disable(bool disable) { + return; + _disabled = disable; + if (_disabled) + _write_pwm(0); +} + +void RcServo::set_homing_mode(bool is_homing, bool isHoming) { + float home_pos = 0.0; + + if (!is_homing) + return; + + if (bit_istrue(homing_dir_mask->get(), bit(axis_index))) + home_pos = _position_min; + else + home_pos = _position_max; + + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo set home %d %3.2f", is_homing, home_pos); + sys_position[axis_index] = home_pos * axis_settings[axis_index]->steps_per_mm->get(); // convert to steps + +} + +void RcServo::update() { + set_location(); +} + +void RcServo::set_location() { + uint32_t servo_pulse_len; + float servo_pos, mpos, offset; + // skip location if we are in alarm mode + + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "locate"); + _get_calibration(); + + if (sys.state == STATE_ALARM) { + set_disable(true); + return; + } + + mpos = system_convert_axis_steps_to_mpos(sys_position, axis_index); // get the axis machine position in mm + offset = gc_state.coord_system[axis_index] + gc_state.coord_offset[axis_index]; // get the current axis work offset + servo_pos = mpos - offset; // determine the current work position + + // determine the pulse length + servo_pulse_len = (uint32_t)mapConstrain(servo_pos, _position_min, _position_max, _pwm_pulse_min, _pwm_pulse_max); + + _write_pwm(servo_pulse_len); + +} + +void RcServo::read_settings() { + _get_calibration(); +} + +// this should change to use its own settings. +void RcServo::_get_calibration() { + float _cal_min = 1.0; + float _cal_max = 1.0; + + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Read settings"); + + // make sure the min is in range + if ((axis_settings[axis_index]->steps_per_mm->get() < SERVO_CAL_MIN) || (axis_settings[axis_index]->steps_per_mm->get() > SERVO_CAL_MAX)) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo calibration ($10%d) value error. Reset to 100", axis_index); + char reset_val[] = "100"; + axis_settings[axis_index]->steps_per_mm->setStringValue(reset_val); + } + + // make sure the max is in range + // Note: Max travel is set positive via $$, but stored as a negative number + if ((axis_settings[axis_index]->max_travel->get() < SERVO_CAL_MIN) || (axis_settings[axis_index]->max_travel->get() > SERVO_CAL_MAX)) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo calibration ($13%d) value error. %3.2f Reset to 100", axis_index, axis_settings[axis_index]->max_travel->get()); + char reset_val[] = "100"; + axis_settings[axis_index]->max_travel->setStringValue(reset_val); + } + + _pwm_pulse_min = SERVO_MIN_PULSE; + _pwm_pulse_max = SERVO_MAX_PULSE; + + + if (bit_istrue(dir_invert_mask->get(), bit(axis_index))) { // normal direction + _cal_min = 2.0 - (axis_settings[axis_index]->steps_per_mm->get() / 100.0); + _cal_max = 2.0 - (axis_settings[axis_index]->max_travel->get() / 100.0); + swap(_pwm_pulse_min, _pwm_pulse_max); + } else { // inverted direction + _cal_min = (axis_settings[axis_index]->steps_per_mm->get() / 100.0); + _cal_max = (axis_settings[axis_index]->max_travel->get() / 100.0); + } + + _pwm_pulse_min *= _cal_min; + _pwm_pulse_max *= _cal_max; + + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo calibration min:%1.2f max %1.2f", _pwm_pulse_min, _pwm_pulse_max); + +} \ No newline at end of file diff --git a/Grbl_Esp32/Motors/RcServoClass.h b/Grbl_Esp32/Motors/RcServoClass.h new file mode 100644 index 00000000..0a23d6ae --- /dev/null +++ b/Grbl_Esp32/Motors/RcServoClass.h @@ -0,0 +1,47 @@ +/* + RcServoClass.h + + Part of Grbl_ESP32 + + 2020 - Bart Dring + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ +#ifndef RCSERVOCLASS_H +#define RCSERVOCLASS_H + +// this is the pulse range of a the servo. Typical servos are 0.001 to 0.002 seconds +// some servos have a wider range. You can adjust this here or in the calibration feature +#define SERVO_MIN_PULSE_SEC 0.001 // min pulse in seconds +#define SERVO_MAX_PULSE_SEC 0.002 // max pulse in seconds + +#define SERVO_POSITION_MIN_DEFAULT 0.0 // mm +#define SERVO_POSITION_MAX_DEFAULT 20.0 // mm + +#define SERVO_PULSE_FREQ 50 // 50Hz ...This is a standard analog servo value. Digital ones can repeat faster + +#define SERVO_PULSE_RES_BITS 16 // bits of resolution of PWM (16 is max) +#define SERVO_PULSE_RES_COUNT 65535 // see above TODO...do the math here 2^SERVO_PULSE_RES_BITS + +#define SERVO_TIME_PER_BIT ((1.0 / (float)SERVO_PULSE_FREQ) / ((float)SERVO_PULSE_RES_COUNT) ) // seconds + +#define SERVO_MIN_PULSE (uint16_t)(SERVO_MIN_PULSE_SEC / SERVO_TIME_PER_BIT) // in timer counts +#define SERVO_MAX_PULSE (uint16_t)(SERVO_MAX_PULSE_SEC / SERVO_TIME_PER_BIT) // in timer counts + +#define SERVO_PULSE_RANGE (SERVO_MAX_PULSE-SERVO_MIN_PULSE) + +#define SERVO_CAL_MIN 20.0 // Percent: the minimum allowable calibration value +#define SERVO_CAL_MAX 180.0 // Percent: the maximum allowable calibration value + +#define SERVO_TIMER_INT_FREQ 50.0 // Hz This is the task frequency + +#endif \ No newline at end of file diff --git a/Grbl_Esp32/Motors/StandardStepperClass.cpp b/Grbl_Esp32/Motors/StandardStepperClass.cpp new file mode 100644 index 00000000..f4f63820 --- /dev/null +++ b/Grbl_Esp32/Motors/StandardStepperClass.cpp @@ -0,0 +1,107 @@ +/* + StandardStepperClass.cpp + + This is used for a stepper motor that just requires step and direction + pins. + TODO: Add an enable pin + + Part of Grbl_ESP32 + + 2020 - Bart Dring + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +StandardStepper :: StandardStepper() { + +} + +StandardStepper :: StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin) { + type_id = STANDARD_MOTOR; + this->axis_index = axis_index % MAX_AXES; + this->dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged + this->step_pin = step_pin; + this->dir_pin = dir_pin; + this->disable_pin = disable_pin; + init(); +} + +void StandardStepper :: init() { + _homing_mask = 0; + is_active = true; // as opposed to NullMotors, this is a real motor + set_axis_name(); + init_step_dir_pins(); + config_message(); +} + +void StandardStepper :: init_step_dir_pins() { + // TODO Step pin, but RMT complicates things + _invert_step_pin = bit_istrue(step_invert_mask->get(), bit(axis_index)); + pinMode(dir_pin, OUTPUT); + +#ifdef USE_RMT_STEPS + rmtConfig.rmt_mode = RMT_MODE_TX; + rmtConfig.clk_div = 20; + rmtConfig.mem_block_num = 2; + rmtConfig.tx_config.loop_en = false; + rmtConfig.tx_config.carrier_en = false; + rmtConfig.tx_config.carrier_freq_hz = 0; + rmtConfig.tx_config.carrier_duty_percent = 50; + rmtConfig.tx_config.carrier_level = RMT_CARRIER_LEVEL_LOW; + rmtConfig.tx_config.idle_output_en = true; + + +#ifdef STEP_PULSE_DELAY + rmtItem[0].duration0 = STEP_PULSE_DELAY * 4; +#else + rmtItem[0].duration0 = 1; +#endif + + rmtItem[0].duration1 = 4 * pulse_microseconds->get(); + rmtItem[1].duration0 = 0; + rmtItem[1].duration1 = 0; + + rmt_chan_num[axis_index][dual_axis_index] = sys_get_next_RMT_chan_num(); + rmt_set_source_clk((rmt_channel_t)rmt_chan_num[axis_index][dual_axis_index], RMT_BASECLK_APB); + rmtConfig.channel = (rmt_channel_t)rmt_chan_num[axis_index][dual_axis_index]; + rmtConfig.tx_config.idle_level = _invert_step_pin ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; + rmtConfig.gpio_num = gpio_num_t(step_pin); // c is a wacky lang + rmtItem[0].level0 = rmtConfig.tx_config.idle_level; + rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; + rmt_config(&rmtConfig); + rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); + +#else + pinMode(step_pin, OUTPUT); + +#endif // USE_RMT_STEPS + pinMode(disable_pin, OUTPUT); +} + + +void StandardStepper :: config_message() { + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "%s Axis standard stepper motor Step:%s Dir:%s Disable:%s", + _axis_name, + pinName(step_pin).c_str(), + pinName(dir_pin).c_str(), + pinName(disable_pin).c_str()); +} + +void StandardStepper :: set_direction_pins(uint8_t onMask) { + digitalWrite(dir_pin, (onMask & bit(axis_index))); +} + +void StandardStepper :: set_disable(bool disable) { + digitalWrite(disable_pin, disable); +} diff --git a/Grbl_Esp32/Motors/TrinamicDriverClass.cpp b/Grbl_Esp32/Motors/TrinamicDriverClass.cpp new file mode 100644 index 00000000..13628f90 --- /dev/null +++ b/Grbl_Esp32/Motors/TrinamicDriverClass.cpp @@ -0,0 +1,242 @@ +/* + TrinamicDriverClass.cpp + This is used for Trinamic SPI controlled stepper motor drivers. + + Part of Grbl_ESP32 + 2020 - Bart Dring + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . + +*/ +#include +#include "TrinamicDriverClass.h" + +TrinamicDriver :: TrinamicDriver(uint8_t axis_index, + uint8_t step_pin, + uint8_t dir_pin, + uint8_t disable_pin, + uint8_t cs_pin, + uint16_t driver_part_number, + float r_sense, + int8_t spi_index) { + type_id = TRINAMIC_SPI_MOTOR; + this->axis_index = axis_index % MAX_AXES; + this->dual_axis_index = axis_index < 6 ? 0 : 1; // 0 = primary 1 = ganged + _driver_part_number = driver_part_number; + _r_sense = r_sense; + this->step_pin = step_pin; + this->dir_pin = dir_pin; + this->disable_pin = disable_pin; + this->cs_pin = cs_pin; + this->spi_index = spi_index; + + _homing_mode = TRINAMIC_HOMING_MODE; + _homing_mask = 0; // no axes homing + + 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); + return; + } + + set_axis_name(); + + init_step_dir_pins(); // from StandardStepper + + digitalWrite(cs_pin, HIGH); + pinMode(cs_pin, OUTPUT); + + // use slower speed if I2S + if (cs_pin >= I2S_OUT_PIN_BASE) + tmcstepper->setSPISpeed(TRINAMIC_SPI_FREQ); + + config_message(); + + // init() must be called later, after all TMC drivers have CS pins setup. +} + +void TrinamicDriver :: init() { + + SPI.begin(); // this will get called for each motor, but does not seem to hurt anything + + tmcstepper->begin(); + test(); // Try communicating with motor. Prints an error if there is a problem. + read_settings(); // pull info from settings + set_mode(false); + + _homing_mask = 0; + is_active = true; // as opposed to NullMotors, this is a real motor +} + +/* + This is the startup message showing the basic definition +*/ +void TrinamicDriver :: config_message() { + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "%s Axis Trinamic TMC%d Step:%s Dir:%s CS:%s Disable:%s Index:%d", + _axis_name, + _driver_part_number, + pinName(step_pin).c_str(), + pinName(dir_pin).c_str(), + pinName(cs_pin).c_str(), + pinName(disable_pin).c_str(), + spi_index); +} + +bool TrinamicDriver :: test() { + switch (tmcstepper->test_connection()) { + case 1: + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Trinamic driver test failed. Check connection", _axis_name); + return false; + case 2: + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Trinamic driver test failed. Check motor power", _axis_name); + return false; + default: + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Trinamic driver test passed", _axis_name); + return true; + } +} + + +/* + Read setting and send them to the driver. Called at init() and whenever related settings change + both are stored as float Amps, but TMCStepper library expects... + uint16_t run (mA) + float hold (as a percentage of run) +*/ +void TrinamicDriver :: read_settings() { + uint16_t run_i_ma = (uint16_t)(axis_settings[axis_index]->run_current->get() * 1000.0); + float hold_i_percent; + + if (axis_settings[axis_index]->run_current->get() == 0) + hold_i_percent = 0; + else { + hold_i_percent = axis_settings[axis_index]->hold_current->get() / axis_settings[axis_index]->run_current->get(); + if (hold_i_percent > 1.0) + hold_i_percent = 1.0; + } + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Current run %d hold %f", _axis_name, run_i_ma, hold_i_percent); + + tmcstepper->microsteps(axis_settings[axis_index]->microsteps->get()); + tmcstepper->rms_current(run_i_ma, hold_i_percent); + +} + +void TrinamicDriver :: set_homing_mode(uint8_t homing_mask, bool isHoming) { + _homing_mask = homing_mask; + set_mode(isHoming); +} + +/* + There are ton of settings. I'll start by grouping then into modes for now. + Many people will want quiet and stallgaurd homing. Stallguard only run in + Coolstep mode, so it will need to switch to Coolstep when homing +*/ +void TrinamicDriver :: set_mode(bool isHoming) { + + if (isHoming) + _mode = TRINAMIC_HOMING_MODE; + else + _mode = TRINAMIC_RUN_MODE; + + if (_lastMode == _mode) + return; + _lastMode = _mode; + + switch (_mode) { + case TRINAMIC_MODE_STEALTHCHOP: + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "TRINAMIC_MODE_STEALTHCHOP"); + tmcstepper->en_pwm_mode(true); + tmcstepper->pwm_autoscale(true); + tmcstepper->diag1_stall(false); + break; + case TRINAMIC_MODE_COOLSTEP: + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "TRINAMIC_MODE_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, MSG_LEVEL_INFO, "TRINAMIC_MODE_STALLGUARD"); + tmcstepper->en_pwm_mode(false); + tmcstepper->pwm_autoscale(false); + tmcstepper->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); + tmcstepper->THIGH(calc_tstep(homing_feed_rate->get(), 60.0)); + tmcstepper->sfilt(1); + tmcstepper->diag1_stall(true); // stallguard i/o is on diag1 + tmcstepper->sgt(axis_settings[axis_index]->stallguard->get()); + break; + default: + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "TRINAMIC_MODE_UNDEFINED"); + } + +} + +/* + This is the stallguard tuning info. It is call debug, so it could be generic across all classes. +*/ +void TrinamicDriver :: debug_message() { + + uint32_t tstep = tmcstepper->TSTEP(); + + if (tstep == 0xFFFFF || tstep < 1) // if axis is not moving return + return; + float feedrate = st_get_realtime_rate(); //* settings.microsteps[axis_index] / 60.0 ; // convert mm/min to Hz + + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "%s Stallguard %d SG_Val: %04d Rate: %05.0f mm/min SG_Setting:%d", + _axis_name, + tmcstepper->stallguard(), + tmcstepper->sg_result(), + feedrate, + axis_settings[axis_index]->stallguard->get()); +} + +// calculate a tstep from a rate +// tstep = TRINAMIC_FCLK / (time between 1/256 steps) +// This is used to set the stallguard window from the homing speed. +// The percent is the offset on the window +uint32_t TrinamicDriver :: calc_tstep(float speed, float percent) { + float tstep = speed / 60.0 * axis_settings[axis_index]->steps_per_mm->get() * (float)(256 / axis_settings[axis_index]->microsteps->get()); + tstep = TRINAMIC_FCLK / tstep * percent / 100.0; + + return (uint32_t)tstep; +} + + +// this can use the enable feature over SPI. The dedicated pin must be in the enable mode, +// but that can be hardwired that way. +void TrinamicDriver :: set_disable(bool disable) { + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Axis disable %d", _axis_name, disable); + + digitalWrite(disable_pin, disable); + +#ifdef USE_TRINAMIC_ENABLE + if (disable) + tmcstepper->toff(TRINAMIC_TOFF_DISABLE); + else { + if (_mode == TRINAMIC_MODE_STEALTHCHOP) + tmcstepper->toff(TRINAMIC_TOFF_STEALTHCHOP); + else + tmcstepper->toff(TRINAMIC_TOFF_COOLSTEP); + } +#endif + // the pin based enable could be added here. + // This would be for individual motors, not the single pin for all motors. +} + diff --git a/Grbl_Esp32/Motors/TrinamicDriverClass.h b/Grbl_Esp32/Motors/TrinamicDriverClass.h new file mode 100644 index 00000000..b31e2195 --- /dev/null +++ b/Grbl_Esp32/Motors/TrinamicDriverClass.h @@ -0,0 +1,64 @@ +/* + TrinamicDriverClass.h + + Part of Grbl_ESP32 + + 2020 - Bart Dring + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#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 NORMAL_TCOOLTHRS 0xFFFFF // 20 bit is max +#define NORMAL_THIGH 0 + +#define TMC2130_RSENSE_DEFAULT 0.11f +#define TMC5160_RSENSE_DEFAULT 0.075f + +#define TRINAMIC_SPI_FREQ 100000 + +#define 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 +#endif + +#ifndef TRINAMIC_HOMING_MODE + #define TRINAMIC_HOMING_MODE TRINAMIC_RUN_MODE +#endif + + +#ifndef TRINAMIC_TOFF_DISABLE + #define TRINAMIC_TOFF_DISABLE 0 +#endif + +#ifndef TRINAMIC_TOFF_STEALTHCHOP + #define TRINAMIC_TOFF_STEALTHCHOP 5 +#endif + +#ifndef TRINAMIC_TOFF_COOLSTEP + #define TRINAMIC_TOFF_COOLSTEP 3 +#endif + + + +#ifndef TRINAMICDRIVERCLASS_H +#define TRINAMICDRIVERCLASS_H + +#include "MotorClass.h" +#include // https://github.com/teemuatlut/TMCStepper + +#endif diff --git a/Grbl_Esp32/Motors/UnipolarMotorClass.cpp b/Grbl_Esp32/Motors/UnipolarMotorClass.cpp new file mode 100644 index 00000000..cd00218c --- /dev/null +++ b/Grbl_Esp32/Motors/UnipolarMotorClass.cpp @@ -0,0 +1,146 @@ +UnipolarMotor :: UnipolarMotor() { + +} + + +UnipolarMotor :: UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3) { + type_id = UNIPOLAR_MOTOR; + this->axis_index = axis_index % MAX_AXES; + this->dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged + _pin_phase0 = pin_phase0; + _pin_phase1 = pin_phase1; + _pin_phase2 = pin_phase2; + _pin_phase3 = pin_phase3; + + _half_step = true; // TODO read from settings ... microstep > 1 = half step + + set_axis_name(); + init(); + config_message(); +} + +void UnipolarMotor :: init() { + pinMode(_pin_phase0, OUTPUT); + pinMode(_pin_phase1, OUTPUT); + pinMode(_pin_phase2, OUTPUT); + pinMode(_pin_phase3, OUTPUT); + _current_phase = 0; +} + +void UnipolarMotor :: config_message() { + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "%s Axis unipolar stepper motor Ph0:%s Ph1:%s Ph2:%s Ph3:%s", + _axis_name, + pinName(_pin_phase0).c_str(), + pinName(_pin_phase1).c_str(), + pinName(_pin_phase2).c_str(), + pinName(_pin_phase3).c_str()); +} + +void UnipolarMotor :: set_disable(bool disable) { + if (disable) { + digitalWrite(_pin_phase0, 0); + digitalWrite(_pin_phase1, 0); + digitalWrite(_pin_phase2, 0); + digitalWrite(_pin_phase3, 0); + } + _enabled = !disable; +} + +void UnipolarMotor::step(uint8_t step_mask, uint8_t dir_mask) { + uint8_t _phase[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // temporary phase values...all start as off + uint8_t phase_max; + + if (!(step_mask & bit(axis_index))) + return; // a step is not required on this interrupt + + if (!_enabled) + return; // don't do anything, phase is not changed or lost + + if (_half_step) + phase_max = 7; + else + phase_max = 3; + + if (dir_mask & bit(axis_index)) { // 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 + + 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]); +} diff --git a/Grbl_Esp32/Pins.cpp b/Grbl_Esp32/Pins.cpp new file mode 100644 index 00000000..a07c929b --- /dev/null +++ b/Grbl_Esp32/Pins.cpp @@ -0,0 +1,55 @@ +#include "grbl.h" +#include "i2s_out.h" + +String pinName(uint8_t pin) { + if (pin == UNDEFINED_PIN) { + return "None"; + } + if (pin < I2S_OUT_PIN_BASE) { + return String("GPIO(") + pin + ")"; + } else { + return String("I2SO(") + (pin - I2S_OUT_PIN_BASE) + ")"; + } +} + +// Even if USE_I2S_OUT is not defined, it is necessary to +// override the following functions, instead of allowing +// the weak aliases in the library to apply, because of +// the UNDEFINED_PIN check. That UNDEFINED_PIN behavior +// cleans up other code by eliminating ifdefs and checks. +void IRAM_ATTR digitalWrite(uint8_t pin, uint8_t val) { + if (pin == UNDEFINED_PIN) { + return; + } + if (pin < I2S_OUT_PIN_BASE) { + __digitalWrite(pin, val); + return; + } +#ifdef USE_I2S_OUT + i2s_out_write(pin - I2S_OUT_PIN_BASE, val); +#endif +} + +void IRAM_ATTR pinMode(uint8_t pin, uint8_t mode) { + if (pin == UNDEFINED_PIN) { + return; + } + if (pin < I2S_OUT_PIN_BASE) + __pinMode(pin, mode); + // I2S out pins cannot be configured, hence there + // is nothing to do here for them. +} + +int IRAM_ATTR digitalRead(uint8_t pin) { + if (pin == UNDEFINED_PIN) { + return 0; + } + if (pin < I2S_OUT_PIN_BASE) { + return __digitalRead(pin); + } +#ifdef USE_I2S_OUT + return i2s_out_state(pin - I2S_OUT_PIN_BASE); +#else + return 0; +#endif +} diff --git a/Grbl_Esp32/Pins.h b/Grbl_Esp32/Pins.h new file mode 100644 index 00000000..4edb86e0 --- /dev/null +++ b/Grbl_Esp32/Pins.h @@ -0,0 +1,12 @@ +#pragma once +#include "Arduino.h" + +#define UNDEFINED_PIN 255 // Can be used to show a pin has no i/O assigned + +#define I2S_OUT_PIN_BASE 128 + +extern "C" int __digitalRead(uint8_t pin); +extern "C" void __pinMode(uint8_t pin, uint8_t mode); +extern "C" void __digitalWrite(uint8_t pin, uint8_t val); + +String pinName(uint8_t pin); diff --git a/Grbl_Esp32/ProcessSettings.cpp b/Grbl_Esp32/ProcessSettings.cpp new file mode 100644 index 00000000..a72cdc72 --- /dev/null +++ b/Grbl_Esp32/ProcessSettings.cpp @@ -0,0 +1,624 @@ +#include "grbl.h" +#include + +// WG Readable and writable as guest +// WU Readable and writable as user and admin +// WA Readable as user and admin, writable as admin + +// If authentication is disabled, auth_level will be LEVEL_ADMIN +bool auth_failed(Word* w, const char* value, auth_t auth_level) { + permissions_t permissions = w->getPermissions(); + switch (auth_level) { + case LEVEL_ADMIN: // Admin can do anything + return false; // Nothing is an Admin auth fail + case LEVEL_GUEST: // Guest can only access open settings + return permissions != WG; // Anything other than RG is Guest auth fail + case LEVEL_USER: // User is complicated... + if (!value) { // User can read anything + return false; // No read is a User auth fail + } + return permissions == WA; // User cannot write WA + default: + return true; + } +} + +void show_setting(const char* name, const char* value, const char* description, ESPResponseStream* out) { + grbl_sendf(out->client(), "$%s=%s", name, value); + if (description) { + grbl_sendf(out->client(), " %s", description); + } + grbl_sendf(out->client(), "\r\n"); +} + +void settings_restore(uint8_t restore_flag) { + #ifdef WIFI_OR_BLUETOOTH + if (restore_flag & SETTINGS_RESTORE_WIFI_SETTINGS) { + #ifdef ENABLE_WIFI + wifi_config.reset_settings(); + #endif + #ifdef ENABLE_BLUETOOTH + bt_config.reset_settings(); + #endif + } + #endif + if (restore_flag & SETTINGS_RESTORE_DEFAULTS) { + for (Setting *s = Setting::List; s; s = s->next()) { + bool restore_startup = restore_flag & SETTINGS_RESTORE_STARTUP_LINES; + if (!s->getDescription()) { + const char *name = s->getName(); + if (restore_startup || ((strcmp(name, "N0") != 0) && (strcmp(name, "N1") == 0))) { + s->setDefault(); + } + } + } + } + if (restore_flag & SETTINGS_RESTORE_PARAMETERS) { + uint8_t idx; + float coord_data[N_AXIS]; + memset(&coord_data, 0, sizeof(coord_data)); + for (idx = 0; idx <= SETTING_INDEX_NCOORD; idx++) settings_write_coord_data(idx, coord_data); + } + if (restore_flag & SETTINGS_RESTORE_BUILD_INFO) { + EEPROM.write(EEPROM_ADDR_BUILD_INFO, 0); + EEPROM.write(EEPROM_ADDR_BUILD_INFO + 1, 0); // Checksum + EEPROM.commit(); + } +} + +// Get settings values from non volatile storage into memory +void load_settings() +{ + for (Setting *s = Setting::List; s; s = s->next()) { + s->load(); + } +} + +extern void make_settings(); +extern void make_grbl_commands(); +extern void make_web_settings(); +void settings_init() +{ + EEPROM.begin(EEPROM_SIZE); + make_settings(); + make_web_settings(); + make_grbl_commands(); + load_settings(); +} + +// TODO Settings - jog may need to be special-cased in the parser, since +// it is not really a setting and the entire line needs to be +// sent to gc_execute_line. It is probably also more time-critical +// than actual settings, which change infrequently, so handling +// it early is probably prudent. +uint8_t jog_set(uint8_t *value, auth_t auth_level, ESPResponseStream* out) { + // Execute only if in IDLE or JOG states. + if (sys.state != STATE_IDLE && sys.state != STATE_JOG) return STATUS_IDLE_ERROR; + + // restore the $J= prefix because gc_execute_line() expects it +#define MAXLINE 128 + char line[MAXLINE]; + strcpy(line, "$J="); + strncat(line, (char *)value, MAXLINE-strlen("$J=")-1); + + return gc_execute_line(line, out->client()); // NOTE: $J= is ignored inside g-code parser and used to detect jog motions. +} + +err_t show_grbl_help(const char* value, auth_t auth_level, ESPResponseStream* out) { + report_grbl_help(out->client()); + return STATUS_OK; +} + +err_t report_gcode(const char *value, auth_t auth_level, ESPResponseStream* out) { + report_gcode_modes(out->client()); + return STATUS_OK; +} + +void show_grbl_settings(ESPResponseStream* out, type_t type, bool wantAxis) { + for (Setting *s = Setting::List; s; s = s->next()) { + if (s->getType() == type && s->getGrblName()) { + bool isAxis = s->getAxis() != NO_AXIS; + // The following test could be expressed more succinctly with XOR, + // but is arguably clearer when written out + if ((wantAxis && isAxis) || (!wantAxis && !isAxis)) { + show_setting(s->getGrblName(), s->getCompatibleValue(), NULL, out); + } + } + } +} +err_t report_normal_settings(const char* value, auth_t auth_level, ESPResponseStream* out) { + show_grbl_settings(out, GRBL, false); // GRBL non-axis settings + show_grbl_settings(out, GRBL, true); // GRBL axis settings + return STATUS_OK; +} +err_t report_extended_settings(const char* value, auth_t auth_level, ESPResponseStream* out) { + show_grbl_settings(out, GRBL, false); // GRBL non-axis settings + show_grbl_settings(out, EXTENDED, false); // Extended non-axis settings + show_grbl_settings(out, GRBL, true); // GRBL axis settings + show_grbl_settings(out, EXTENDED, true); // Extended axis settings + return STATUS_OK; +} +err_t list_grbl_names(const char* value, auth_t auth_level, ESPResponseStream* out) +{ + for (Setting *s = Setting::List; s; s = s->next()) { + const char* gn = s->getGrblName(); + if (gn) { + grbl_sendf(out->client(), "$%s => $%s\r\n", gn, s->getName()); + } + } + return STATUS_OK; +} +err_t list_settings(const char* value, auth_t auth_level, ESPResponseStream* out) +{ + for (Setting *s = Setting::List; s; s = s->next()) { + const char *displayValue = auth_failed(s, value, auth_level) + ? "" + : s->getStringValue(); + show_setting(s->getName(), displayValue, NULL, out); + } + return STATUS_OK; +} +err_t list_commands(const char* value, auth_t auth_level, ESPResponseStream* out) +{ + for (Command *cp = Command::List; cp; cp = cp->next()) { + const char* name = cp->getName(); + const char* oldName = cp->getGrblName(); + if (oldName) { + grbl_sendf(out->client(), "$%s or $%s", name, oldName); + } else { + grbl_sendf(out->client(), "$%s", name); + } + const char* description = cp->getDescription(); + if (description) { + grbl_sendf(out->client(), " =%s", description); + } + grbl_sendf(out->client(), "\r\n"); + } + return STATUS_OK; +} +err_t toggle_check_mode(const char* value, auth_t auth_level, ESPResponseStream* out) { + // Perform reset when toggling off. Check g-code mode should only work if Grbl + // is idle and ready, regardless of alarm locks. This is mainly to keep things + // simple and consistent. + if (sys.state == STATE_CHECK_MODE) { + mc_reset(); + report_feedback_message(MESSAGE_DISABLED); + } else { + if (sys.state) return (STATUS_IDLE_ERROR); // Requires no alarm mode. + sys.state = STATE_CHECK_MODE; + report_feedback_message(MESSAGE_ENABLED); + } + return STATUS_OK; +} +err_t disable_alarm_lock(const char* value, auth_t auth_level, ESPResponseStream* out) { + if (sys.state == STATE_ALARM) { + // Block if safety door is ajar. + if (system_check_safety_door_ajar()) + return (STATUS_CHECK_DOOR); + report_feedback_message(MESSAGE_ALARM_UNLOCK); + sys.state = STATE_IDLE; + // Don't run startup script. Prevents stored moves in startup from causing accidents. + } // Otherwise, no effect. + return STATUS_OK; +} +err_t report_ngc(const char* value, auth_t auth_level, ESPResponseStream* out) { + report_ngc_parameters(out->client()); + return STATUS_OK; +} +err_t home(int cycle) { + if (homing_enable->get() == false) + return (STATUS_SETTING_DISABLED); + if (system_check_safety_door_ajar()) + return (STATUS_CHECK_DOOR); // Block if safety door is ajar. + sys.state = STATE_HOMING; // Set system state variable + mc_homing_cycle(cycle); + if (!sys.abort) { // Execute startup scripts after successful homing. + sys.state = STATE_IDLE; // Set to IDLE when complete. + st_go_idle(); // Set steppers to the settings idle state before returning. + if (cycle == HOMING_CYCLE_ALL) { + char line[128]; + system_execute_startup(line); + } + } + return STATUS_OK; +} +err_t home_all(const char* value, auth_t auth_level, ESPResponseStream* out) { + return home(HOMING_CYCLE_ALL); +} +err_t home_x(const char* value, auth_t auth_level, ESPResponseStream* out) { + return home(HOMING_CYCLE_X); +} +err_t home_y(const char* value, auth_t auth_level, ESPResponseStream* out) { + return home(HOMING_CYCLE_Y); +} +err_t home_z(const char* value, auth_t auth_level, ESPResponseStream* out) { + return home(HOMING_CYCLE_Z); +} +err_t home_a(const char* value, auth_t auth_level, ESPResponseStream* out) { + return home(HOMING_CYCLE_A); +} +err_t home_b(const char* value, auth_t auth_level, ESPResponseStream* out) { + return home(HOMING_CYCLE_B); +} +err_t home_c(const char* value, auth_t auth_level, ESPResponseStream* out) { + return home(HOMING_CYCLE_C); +} +err_t sleep_grbl(const char* value, auth_t auth_level, ESPResponseStream* out) { + system_set_exec_state_flag(EXEC_SLEEP); + return STATUS_OK; +} +err_t get_report_build_info(const char* value, auth_t auth_level, ESPResponseStream* out) { + if (!value) { + char line[128]; + settings_read_build_info(line); + report_build_info(line, out->client()); + return STATUS_OK; + } + #ifdef ENABLE_BUILD_INFO_WRITE_COMMAND + settings_store_build_info(value); + return STATUS_OK; + #else + return STATUS_INVALID_STATEMENT; + #endif +} +err_t report_startup_lines(const char* value, auth_t auth_level, ESPResponseStream* out) { + report_startup_line(0, startup_line_0->get(), out->client()); + report_startup_line(1, startup_line_1->get(), out->client()); + return STATUS_OK; +} + +std::map restoreCommands = { + #ifdef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS + { "$", SETTINGS_RESTORE_DEFAULTS }, + { "settings", SETTINGS_RESTORE_DEFAULTS }, + #endif + #ifdef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS + { "#", SETTINGS_RESTORE_PARAMETERS }, + { "gcode", SETTINGS_RESTORE_PARAMETERS }, + #endif + #ifdef ENABLE_RESTORE_EEPROM_WIPE_ALL + { "*", SETTINGS_RESTORE_ALL }, + { "all", SETTINGS_RESTORE_ALL }, + #endif + { "@", SETTINGS_RESTORE_WIFI_SETTINGS }, + { "wifi", SETTINGS_RESTORE_WIFI_SETTINGS }, +}; +err_t restore_settings(const char* value, auth_t auth_level, ESPResponseStream* out) { + if (!value) { + return STATUS_INVALID_STATEMENT; + } + auto it = restoreCommands.find(value); + if (it == restoreCommands.end()) { + return STATUS_INVALID_STATEMENT; + } + settings_restore(it->second); + return STATUS_OK; +} + +err_t showState(const char* value, auth_t auth_level, ESPResponseStream* out) { + grbl_sendf(out->client(), "State 0x%x\r\n", sys.state); + return STATUS_OK; +} +err_t doJog(const char* value, auth_t auth_level, ESPResponseStream* out) { + // For jogging, you must give gc_execute_line() a line that + // begins with $J=. There are several ways we can get here, + // including $J, $J=xxx, [J]xxx. For any form other than + // $J without =, we reconstruct a $J= line for gc_execute_line(). + if (!value) { + return STATUS_INVALID_STATEMENT; + } + char jogLine[LINE_BUFFER_SIZE]; + strcpy(jogLine, "$J="); + strcat(jogLine, value); + return gc_execute_line(jogLine, out->client()); +} + +std::map ErrorCodes = { + { STATUS_OK , "No error", }, + { STATUS_EXPECTED_COMMAND_LETTER , "Expected GCodecommand letter", }, + { STATUS_BAD_NUMBER_FORMAT , "Bad GCode number format", }, + { STATUS_INVALID_STATEMENT , "Invalid $ statement", }, + { STATUS_NEGATIVE_VALUE , "Negative value", }, + { STATUS_SETTING_DISABLED , "Setting disabled", }, + { STATUS_SETTING_STEP_PULSE_MIN , "Step pulse too short", }, + { STATUS_SETTING_READ_FAIL , "Failed to read settings", }, + { STATUS_IDLE_ERROR , "Command requires idle state", }, + { STATUS_SYSTEM_GC_LOCK , "GCode cannot be executed in lock or alarm state", }, + { STATUS_SOFT_LIMIT_ERROR , "Soft limit error", }, + { STATUS_OVERFLOW , "Line too long", }, + { STATUS_MAX_STEP_RATE_EXCEEDED , "Max step rate exceeded", }, + { STATUS_CHECK_DOOR , "Check door", }, + { STATUS_LINE_LENGTH_EXCEEDED , "Startup line too long", }, + { STATUS_TRAVEL_EXCEEDED , "Max travel exceeded during jog", }, + { STATUS_INVALID_JOG_COMMAND , "Invalid jog command", }, + { STATUS_SETTING_DISABLED_LASER , "Laser mode requires PWM output", }, + { STATUS_GCODE_UNSUPPORTED_COMMAND , "Unsupported GCode command", }, + { STATUS_GCODE_MODAL_GROUP_VIOLATION , "Gcode modal group violation", }, + { STATUS_GCODE_UNDEFINED_FEED_RATE , "Gcode undefined feed rate", }, + { STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER , "Gcode command value not integer", }, + { STATUS_GCODE_AXIS_COMMAND_CONFLICT , "Gcode axis command conflict", }, + { STATUS_GCODE_WORD_REPEATED , "Gcode word repeated", }, + { STATUS_GCODE_NO_AXIS_WORDS , "Gcode no axis words", }, + { STATUS_GCODE_INVALID_LINE_NUMBER , "Gcode invalid line number", }, + { STATUS_GCODE_VALUE_WORD_MISSING , "Gcode value word missing", }, + { STATUS_GCODE_UNSUPPORTED_COORD_SYS , "Gcode unsupported coordinate system", }, + { STATUS_GCODE_G53_INVALID_MOTION_MODE , "Gcode G53 invalid motion mode", }, + { STATUS_GCODE_AXIS_WORDS_EXIST , "Gcode extra axis words", }, + { STATUS_GCODE_NO_AXIS_WORDS_IN_PLANE , "Gcode no axis words in plane", }, + { STATUS_GCODE_INVALID_TARGET , "Gcode invalid target", }, + { STATUS_GCODE_ARC_RADIUS_ERROR , "Gcode arc radius error", }, + { STATUS_GCODE_NO_OFFSETS_IN_PLANE , "Gcode no offsets in plane", }, + { STATUS_GCODE_UNUSED_WORDS , "Gcode unused words", }, + { STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR , "Gcode G43 dynamic axis error", }, + { STATUS_GCODE_MAX_VALUE_EXCEEDED , "Gcode max value exceeded", }, + { STATUS_P_PARAM_MAX_EXCEEDED , "P param max exceeded", }, + { STATUS_SD_FAILED_MOUNT , "SD failed mount", }, + { STATUS_SD_FAILED_READ , "SD failed read", }, + { STATUS_SD_FAILED_OPEN_DIR , "SD failed to open directory", }, + { STATUS_SD_DIR_NOT_FOUND , "SD directory not found", }, + { STATUS_SD_FILE_EMPTY , "SD file empty", }, + { STATUS_SD_FILE_NOT_FOUND , "SD file not found", }, + { STATUS_SD_FAILED_OPEN_FILE , "SD failed to open file", }, + { STATUS_SD_FAILED_BUSY , "SD is busy", }, + { STATUS_SD_FAILED_DEL_DIR, "SD failed to delete directory", }, + { STATUS_SD_FAILED_DEL_FILE, "SD failed to delete file", }, + { STATUS_BT_FAIL_BEGIN , "Bluetooth failed to start", }, + { STATUS_WIFI_FAIL_BEGIN , "WiFi failed to start", }, + { STATUS_NUMBER_RANGE , "Number out of range for setting", }, + { STATUS_INVALID_VALUE , "Invalid value for setting", }, + { STATUS_MESSAGE_FAILED , "Failed to send message", }, + { STATUS_NVS_SET_FAILED , "Failed to store setting", }, + { STATUS_AUTHENTICATION_FAILED, "Authentication failed!", }, +}; + +const char* errorString(err_t errorNumber) { + auto it = ErrorCodes.find(errorNumber); + return it == ErrorCodes.end() ? NULL : it->second; +} + +err_t listErrorCodes(const char* value, auth_t auth_level, ESPResponseStream* out) { + if (value) { + char* endptr = NULL; + uint8_t errorNumber = strtol(value, &endptr, 10); + if (*endptr) { + grbl_sendf(out->client(), "Malformed error number: %s\r\n", value); + return STATUS_INVALID_VALUE; + } + const char* errorName = errorString(errorNumber); + if (errorName) { + grbl_sendf(out->client(), "%d: %s\r\n", errorNumber, errorName); + return STATUS_OK; + } else { + grbl_sendf(out->client(), "Unknown error number: %d\r\n", errorNumber); + return STATUS_INVALID_VALUE; + } + } + + for (auto it = ErrorCodes.begin(); + it != ErrorCodes.end(); + it++) { + grbl_sendf(out->client(), "%d: %s\r\n", it->first, it->second); + } + return STATUS_OK; +} + + +// Commands use the same syntax as Settings, but instead of setting or +// displaying a persistent value, a command causes some action to occur. +// That action could be anything, from displaying a run-time parameter +// to performing some system state change. Each command is responsible +// for decoding its own value string, if it needs one. +void make_grbl_commands() { + new GrblCommand("", "Help", show_grbl_help, ANY_STATE); + new GrblCommand("T", "State", showState, ANY_STATE); + new GrblCommand("J", "Jog", doJog, IDLE_OR_JOG); + + new GrblCommand("$", "GrblSettings/List", report_normal_settings, NOT_CYCLE_OR_HOLD); + new GrblCommand("+", "ExtendedSettings/List", report_extended_settings, NOT_CYCLE_OR_HOLD); + new GrblCommand("L", "GrblNames/List", list_grbl_names, NOT_CYCLE_OR_HOLD); + new GrblCommand("S", "Settings/List", list_settings, NOT_CYCLE_OR_HOLD); + new GrblCommand("CMD", "Commands/List", list_commands, NOT_CYCLE_OR_HOLD); + new GrblCommand("E", "ErrorCodes/List",listErrorCodes, ANY_STATE); + new GrblCommand("G", "GCode/Modes", report_gcode, ANY_STATE); + new GrblCommand("C", "GCode/Check", toggle_check_mode, ANY_STATE); + new GrblCommand("X", "Alarm/Disable", disable_alarm_lock, ANY_STATE); + new GrblCommand("NVX", "Settings/Erase", Setting::eraseNVS, IDLE_OR_ALARM, WA); + new GrblCommand("V", "Settings/Stats", Setting::report_nvs_stats, IDLE_OR_ALARM); + new GrblCommand("#", "GCode/Offsets", report_ngc, IDLE_OR_ALARM); + new GrblCommand("H", "Home", home_all, IDLE_OR_ALARM); + #ifdef HOMING_SINGLE_AXIS_COMMANDS + new GrblCommand("HX", "Home/X", home_x, IDLE_OR_ALARM); + new GrblCommand("HY", "Home/Y", home_y, IDLE_OR_ALARM); + new GrblCommand("HZ", "Home/Z", home_z, IDLE_OR_ALARM); + #if (N_AXIS > 3) + new GrblCommand("HA", "Home/A", home_a, IDLE_OR_ALARM); + #endif + #if (N_AXIS > 4) + new GrblCommand("HB", "Home/B", home_b, IDLE_OR_ALARM); + #endif + #if (N_AXIS > 5) + new GrblCommand("HC", "Home/C", home_c, IDLE_OR_ALARM); + #endif + #endif + new GrblCommand("SLP", "System/Sleep", sleep_grbl, IDLE_OR_ALARM); + new GrblCommand("I", "Build/Info", get_report_build_info, IDLE_OR_ALARM); + new GrblCommand("N", "GCode/StartupLines", report_startup_lines, IDLE_OR_ALARM); + new GrblCommand("RST", "Settings/Restore", restore_settings, IDLE_OR_ALARM, WA); +}; + +// normalize_key puts a key string into canonical form - +// without whitespace. +// start points to a null-terminated string. +// Returns the first substring that does not contain whitespace. +// Case is unchanged because comparisons are case-insensitive. +char *normalize_key(char *start) { + char c; + + // In the usual case, this loop will exit on the very first test, + // because the first character is likely to be non-white. + // Null ('\0') is not considered to be a space character. + while (isspace(c = *start) && c != '\0') { + ++start; + } + + // start now points to either a printable character or end of string + if (c == '\0') { + return start; + } + + // Having found the beginning of the printable string, + // we now scan forward until we find a space character. + char *end; + for (end = start; (c = *end) != '\0' && !isspace(c); end++) { + } + + // end now points to either a whitespace character of end of string + // In either case it is okay to place a null there + *end = '\0'; + + return start; +} + +// This is the handler for all forms of settings commands, +// $..= and [..], with and without a value. +err_t do_command_or_setting(const char *key, char *value, auth_t auth_level, ESPResponseStream* out) { + // If value is NULL, it means that there was no value string, i.e. + // $key without =, or [key] with nothing following. + // If value is not NULL, but the string is empty, that is the form + // $key= with nothing following the = . It is important to distinguish + // those cases so that you can say "$N0=" to clear a startup line. + + // First search the settings list by text name. If found, set a new + // value if one is given, otherwise display the current value + for (Setting *s = Setting::List; s; s = s->next()) { + if (strcasecmp(s->getName(), key) == 0) { + if (auth_failed(s, value, auth_level)) { + return STATUS_AUTHENTICATION_FAILED; + } + if (value) { + return s->setStringValue(value); + } else { + show_setting(s->getName(), s->getStringValue(), NULL, out); + return STATUS_OK; + } + } + } + + // Then search the setting list by compatible name. If found, set a new + // value if one is given, otherwise display the current value in compatible mode + for (Setting *s = Setting::List; s; s = s->next()) { + if (s->getGrblName() && strcasecmp(s->getGrblName(), key) == 0) { + if (auth_failed(s, value, auth_level)) { + return STATUS_AUTHENTICATION_FAILED; + } + if (value) { + return s->setStringValue(value); + } else { + show_setting(s->getGrblName(), s->getCompatibleValue(), NULL, out); + return STATUS_OK; + } + } + } + // If we did not find a setting, look for a command. Commands + // handle values internally; you cannot determine whether to set + // or display solely based on the presence of a value. + for (Command *cp = Command::List; cp; cp = cp->next()) { + if ( (strcasecmp(cp->getName(), key) == 0) + || (cp->getGrblName() + && strcasecmp(cp->getGrblName(), key) == 0 + ) + ) { + if (auth_failed(cp, value, auth_level)) { + return STATUS_AUTHENTICATION_FAILED; + } + return cp->action(value, auth_level, out); + } + } + + // If we did not find an exact match and there is no value, + // indicating a display operation, we allow partial matches + // and display every possibility. This only applies to the + // text form of the name, not to the nnn and ESPnnn forms. + err_t retval = STATUS_INVALID_STATEMENT; + if (!value) { + auto lcKey = String(key); + // We allow the key string to begin with *, which we remove. + // This lets us look at X axis settings with $*x. + // $x by itself is the disable alarm lock command + if (lcKey.startsWith("*")) { + lcKey.remove(0,1); + } + lcKey.toLowerCase(); + bool found = false; + for (Setting *s = Setting::List; s; s = s->next()) { + auto lcTest = String(s->getName()); + lcTest.toLowerCase(); + + if (lcTest.indexOf(lcKey) >= 0) { + const char *displayValue = auth_failed(s, value, auth_level) + ? "" + : s->getStringValue(); + show_setting(s->getName(), displayValue, NULL, out); + found = true; + } + } + if (found) { + return STATUS_OK; + } + } + return STATUS_INVALID_STATEMENT; +} + +uint8_t system_execute_line(char* line, ESPResponseStream* out, auth_t auth_level) { + remove_password(line, auth_level); + + char *value; + if (*line++ == '[') { // [ESPxxx] form + value = strrchr(line, ']'); + if (!value) { + // Missing ] is an error in this form + return STATUS_INVALID_STATEMENT; + } + // ']' was found; replace it with null and set value to the rest of the line. + *value++ = '\0'; + // If the rest of the line is empty, replace value with NULL. + if (*value == '\0') { + value = NULL; + } + } else { + // $xxx form + value = strchr(line, '='); + if (value) { + // $xxx=yyy form. + *value++ = '\0'; + } + } + + char *key = normalize_key(line); + + // At this point there are three possibilities for value + // NULL - $xxx without = + // NULL - [ESPxxx] with nothing after ] + // empty string - $xxx= with nothing after + // non-empty string - [ESPxxx]yyy or $xxx=yyy + return do_command_or_setting(key, value, auth_level, out); +} +uint8_t system_execute_line(char* line, uint8_t client, auth_t auth_level) { + return system_execute_line(line, new ESPResponseStream(client, true), auth_level); +} + +void system_execute_startup(char* line) { + err_t status_code; + char gcline[256]; + strncpy(gcline, startup_line_0->get(), 255); + if (*gcline) { + status_code = gc_execute_line(gcline, CLIENT_SERIAL); + report_execute_startup_message(gcline, status_code, CLIENT_SERIAL); + } + strncpy(gcline, startup_line_1->get(), 255); + if (*gcline) { + status_code = gc_execute_line(gcline, CLIENT_SERIAL); + report_execute_startup_message(gcline, status_code, CLIENT_SERIAL); + } +} + diff --git a/Grbl_Esp32/SettingsClass.cpp b/Grbl_Esp32/SettingsClass.cpp new file mode 100644 index 00000000..4825aae3 --- /dev/null +++ b/Grbl_Esp32/SettingsClass.cpp @@ -0,0 +1,613 @@ +#include "grbl.h" +#include "JSONencoder.h" +#include +#include "nvs.h" + +Word::Word(type_t type, permissions_t permissions, const char* description, const char* grblName, const char* fullName) + : _description(description) + , _grblName(grblName) + , _fullName(fullName) + , _type(type) + , _permissions(permissions) +{} + +Command* Command::List = NULL; + +Command::Command(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName) + : Word(type, permissions, description, grblName, fullName) +{ + link = List; + List = this; +} + +Setting* Setting::List = NULL; + +Setting::Setting(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName, bool (*checker)(char *)) + : Word(type, permissions, description, grblName, fullName) + , _checker(checker) +{ + link = List; + List = this; + + // NVS keys are limited to 15 characters, so if the setting name is longer + // than that, we derive a 15-character name from a hash function + size_t len = strlen(fullName); + if (len <= 15) { + _keyName = _fullName; + } else { + // This is Donald Knuth's hash function from Vol 3, chapter 6.4 + char *hashName = (char *)malloc(16); + uint32_t hash = len; + for (const char *s = fullName; *s; s++) { + hash = ((hash << 5) ^ (hash >> 27)) ^ (*s); + } + sprintf(hashName, "%.7s%08x", fullName, hash); + _keyName = hashName; + } +} + +err_t Setting::check(char *s) { + if (sys.state != STATE_IDLE && !(sys.state & STATE_ALARM)) { + return STATUS_IDLE_ERROR; + } + if (!_checker) { + return STATUS_OK; + } + return _checker(s) ? STATUS_OK : STATUS_INVALID_VALUE; +} + +nvs_handle Setting::_handle = 0; + +void Setting::init() { + if (!_handle) { + if (esp_err_t err = nvs_open("Grbl_ESP32", NVS_READWRITE, &_handle)) { + grbl_sendf(CLIENT_SERIAL, "nvs_open failed with error %d\r\n", err); + } + } +} + +IntSetting::IntSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, int32_t defVal, int32_t minVal, int32_t maxVal, bool (*checker)(char *) = NULL) + : Setting(description, type, permissions, grblName, name, checker) + , _defaultValue(defVal) + , _currentValue(defVal) + , _minValue(minVal) + , _maxValue(maxVal) +{ } + +void IntSetting::load() { + esp_err_t err = nvs_get_i32(_handle, _keyName, &_storedValue); + if (err) { + _storedValue = std::numeric_limits::min(); + _currentValue = _defaultValue; + } else { + _currentValue = _storedValue; + } +} + +void IntSetting::setDefault() { + _currentValue = _defaultValue; + if (_storedValue != _currentValue) { + nvs_erase_key(_handle, _keyName); + } +} + +err_t IntSetting::setStringValue(char* s) { + s = trim(s); + if (err_t err = check(s)) { + return err; + } + char* endptr; + int32_t convertedValue = strtol(s, &endptr, 10); + if (endptr == s || *endptr != '\0') { + return STATUS_BAD_NUMBER_FORMAT; + } + if (convertedValue < _minValue || convertedValue > _maxValue) { + return STATUS_NUMBER_RANGE; + } + _currentValue = convertedValue; + if (_storedValue != _currentValue) { + if (_currentValue == _defaultValue) { + nvs_erase_key(_handle, _keyName); + } else { + if (nvs_set_i32(_handle, _keyName, _currentValue)) { + return STATUS_NVS_SET_FAILED; + } + _storedValue = _currentValue; + } + } + return STATUS_OK; +} + +const char* IntSetting::getStringValue() { + static char strval[32]; + sprintf(strval, "%d", get()); + return strval; +} + +void IntSetting::addWebui(JSONencoder *j) { + if (getDescription()) { + j->begin_webui(getName(), getDescription(), "I", getStringValue(), _minValue, _maxValue); + j->end_object(); + } +} + +AxisMaskSetting::AxisMaskSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, int32_t defVal, bool (*checker)(char *) = NULL) + : Setting(description, type, permissions, grblName, name, checker) + , _defaultValue(defVal) + , _currentValue(defVal) +{ } + +void AxisMaskSetting::load() { + esp_err_t err = nvs_get_i32(_handle, _keyName, &_storedValue); + if (err) { + _storedValue = -1; + _currentValue = _defaultValue; + } else { + _currentValue = _storedValue; + } +} + +void AxisMaskSetting::setDefault() { + _currentValue = _defaultValue; + if (_storedValue != _currentValue) { + nvs_erase_key(_handle, _keyName); + } +} + +err_t AxisMaskSetting::setStringValue(char* s) { + s = trim(s); + if (err_t err = check(s)) { + return err; + } + int32_t convertedValue; + char* endptr; + if (*s == '\0') { + convertedValue = 0; + } 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 STATUS_BAD_NUMBER_FORMAT; + } + convertedValue |= bit(index); + } + } + } + _currentValue = convertedValue; + if (_storedValue != _currentValue) { + if (_currentValue == _defaultValue) { + nvs_erase_key(_handle, _keyName); + } else { + if (nvs_set_i32(_handle, _keyName, _currentValue)) { + return STATUS_NVS_SET_FAILED; + } + _storedValue = _currentValue; + } + } + return STATUS_OK; +} + +const char* AxisMaskSetting::getCompatibleValue() { + static char strval[32]; + sprintf(strval, "%d", get()); + return strval; +} + +const char* AxisMaskSetting::getStringValue() { + static char strval[32]; + char *s = strval; + uint32_t mask = get(); + for (int i = 0; i < MAX_N_AXIS; i++) { + if (mask & bit(i)) { + *s++ = "XYZABC"[i]; + } + } + *s = '\0'; + return strval; +} + +void AxisMaskSetting::addWebui(JSONencoder *j) { + if (getDescription()) { + j->begin_webui(getName(), getDescription(), "I", getStringValue(), 0, (1<end_object(); + } +} + +FloatSetting::FloatSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, float defVal, float minVal, float maxVal, bool (*checker)(char *) = NULL) + : Setting(description, type, permissions, grblName, name, checker) + , _defaultValue(defVal) + , _currentValue(defVal) + , _minValue(minVal) + , _maxValue(maxVal) +{ } + +void FloatSetting::load() { + union { + int32_t ival; + float fval; + } v; + if (nvs_get_i32(_handle, _keyName, &v.ival)) { + _currentValue = _defaultValue; + } else { + _currentValue = v.fval; + } +} + +void FloatSetting::setDefault() { + _currentValue = _defaultValue; + if (_storedValue != _currentValue) { + nvs_erase_key(_handle, _keyName); + } +} + +err_t FloatSetting::setStringValue(char* s) { + s = trim(s); + if (err_t err = check(s)) { + return err; + } + + float convertedValue; + uint8_t len = strlen(s); + uint8_t retlen = 0; + if (!read_float(s, &retlen, &convertedValue) + || retlen != len) + { + return STATUS_BAD_NUMBER_FORMAT; + } + if (convertedValue < _minValue || convertedValue > _maxValue) { + return STATUS_NUMBER_RANGE; + } + _currentValue = convertedValue; + if (_storedValue != _currentValue) { + if (_currentValue == _defaultValue) { + nvs_erase_key(_handle, _keyName); + } else { + union { + int32_t ival; + float fval; + } v; + v.fval = _currentValue; + if (nvs_set_i32(_handle, _keyName, v.ival)) { + return STATUS_NVS_SET_FAILED; + } + _storedValue = _currentValue; + } + } + return STATUS_OK; +} + +const char* FloatSetting::getStringValue() { + static char strval[32]; + (void)sprintf(strval, "%.3f", get()); +#if 0 + // With the goal of representing both large and small floating point + // numbers compactly while showing clearly that the are floating point, + // remove trailing zeros leaving at least one post-decimal digit. + // The loop is guaranteed to terminate because the string contains + // a decimal point which is not a '0'. + for (char *p = strval + strlen(strval) - 1; *p == '0'; --p) { + if (*(p-1) != '.' && *(p-1) != ',') { + *p = '\0'; + } + } +#endif + return strval; +} + +StringSetting::StringSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, const char* defVal, int min, int max, bool (*checker)(char *)) + : Setting(description, type, permissions, grblName, name, checker) +{ + _defaultValue = defVal; + _currentValue = defVal; + _minLength = min; + _maxLength = max; + }; + +void StringSetting::load() { + size_t len = 0; + esp_err_t err = nvs_get_str(_handle, _keyName, NULL, &len); + if(err) { + _storedValue = _defaultValue; + _currentValue = _defaultValue; + return; + } + char buf[len]; + err = nvs_get_str(_handle, _keyName, buf, &len); + if (err) { + _storedValue = _defaultValue; + _currentValue = _defaultValue; + return; + } + _storedValue = String(buf); + _currentValue = _storedValue; +} + +void StringSetting::setDefault() { + _currentValue = _defaultValue; + if (_storedValue != _currentValue) { + nvs_erase_key(_handle, _keyName); + } +} + +err_t StringSetting::setStringValue(char* s) { + if (_minLength && _maxLength && (strlen(s) < _minLength || strlen(s) > _maxLength)) { + return STATUS_BAD_NUMBER_FORMAT; + } + if (err_t err = check(s)) { + return err; + } + _currentValue = s; + if (_storedValue != _currentValue) { + if (_currentValue == _defaultValue) { + nvs_erase_key(_handle, _keyName); + _storedValue = _defaultValue; + } else { + if (nvs_set_str(_handle, _keyName, _currentValue.c_str())) { + return STATUS_NVS_SET_FAILED; + } + _storedValue = _currentValue; + } + } + return STATUS_OK; +} + +const char* StringSetting::getStringValue() { + // If the string is a password do not display it + if (_checker && + ( + #ifdef ENABLE_WIFI + _checker == (bool (*)(char *))WiFiConfig::isPasswordValid + || + #endif + _checker == (bool (*)(char *))COMMANDS::isLocalPasswordValid + )) { + return "******"; + } + return _currentValue.c_str(); +} + +void StringSetting::addWebui(JSONencoder *j) { + if (!getDescription()) { + return; + } + j->begin_webui( + getName(), getDescription(), "S", getStringValue(), _minLength, _maxLength); + j->end_object(); +} + +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) + , _defaultValue(defVal) + , _options(opts) +{ } + +void EnumSetting::load() { + esp_err_t err = nvs_get_i8(_handle, _keyName, &_storedValue); + if (err) { + _storedValue = -1; + _currentValue = _defaultValue; + } else { + _currentValue = _storedValue; + } +} + +void EnumSetting::setDefault() { + _currentValue = _defaultValue; + if (_storedValue != _currentValue) { + nvs_erase_key(_handle, _keyName); + } +} + +// For enumerations, we allow the value to be set +// either with the string name or the numeric value. +// This is necessary for WebUI, which uses the number +// for setting. +err_t EnumSetting::setStringValue(char* s) { + s = trim(s); + 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 + + // Disallow empty string + if (!s || !*s) { + return STATUS_BAD_NUMBER_FORMAT; + } + char *endptr; + uint8_t num = strtol(s, &endptr, 10); + // Disallow non-numeric characters in string + if (*endptr) { + return STATUS_BAD_NUMBER_FORMAT; + } + for (it = _options->begin(); it != _options->end(); it++) { + if (it->second == num) { + break; + } + } + if (it == _options->end()) { + return STATUS_BAD_NUMBER_FORMAT; + } + } + _currentValue = it->second; + if (_storedValue != _currentValue) { + if (_storedValue == _defaultValue) { + nvs_erase_key(_handle, _keyName); + } else { + if (nvs_set_i8(_handle, _keyName, _currentValue)) { + return STATUS_NVS_SET_FAILED; + } + _storedValue = _currentValue; + } + } + return STATUS_OK; +} + +const char* EnumSetting::getStringValue() { + for (enum_opt_t::iterator it = _options->begin(); + it != _options->end(); + it++) { + if (it->second == _currentValue) { + return it->first; + } + } + return "???"; +} + +void EnumSetting::addWebui(JSONencoder *j) { + if (!getDescription()) { + return; + } + j->begin_webui(getName(), getDescription(), "B", String(get()).c_str()); + j->begin_array("O"); + for (enum_opt_t::iterator it = _options->begin(); + it != _options->end(); + it++) { + j->begin_object(); + j->member(it->first, it->second); + j->end_object(); + } + j->end_array(); + j->end_object(); +} + +FlagSetting::FlagSetting(const char *description, type_t type, permissions_t permissions, const char * grblName, const char* name, bool defVal, bool (*checker)(char *) = NULL) : + Setting(description, type, permissions, grblName, name, checker), + _defaultValue(defVal) +{ } + +void FlagSetting::load() { + esp_err_t err = nvs_get_i8(_handle, _keyName, &_storedValue); + if (err) { + _storedValue = -1; // Neither well-formed false (0) nor true (1) + _currentValue = _defaultValue; + } else { + _currentValue = !!_storedValue; + } +} +void FlagSetting::setDefault() { + _currentValue = _defaultValue; + if (_storedValue != _currentValue) { + nvs_erase_key(_handle, _keyName); + } +} + +err_t FlagSetting::setStringValue(char* s) { + s = trim(s); + _currentValue = (strcasecmp(s, "on") == 0) + || (strcasecmp(s, "true") == 0) + || (strcasecmp(s, "enabled") == 0) + || (strcasecmp(s, "yes") == 0) + || (strcasecmp(s, "1") == 0); + // _storedValue is -1, 0, or 1 + // _currentValue is 0 or 1 + if (_storedValue != (int8_t)_currentValue) { + if (_currentValue == _defaultValue) { + nvs_erase_key(_handle, _keyName); + } else { + if (nvs_set_i8(_handle, _keyName, _currentValue)) { + return STATUS_NVS_SET_FAILED; + } + _storedValue = _currentValue; + } + } + return STATUS_OK; +} +const char* FlagSetting::getStringValue() { + return get() ? "On" : "Off"; +} +const char* FlagSetting::getCompatibleValue() { + return get() ? "1" : "0"; +} + + +#include + +IPaddrSetting::IPaddrSetting(const char *description, type_t type, permissions_t permissions, const char * grblName, const char* name, uint32_t defVal, bool (*checker)(char *) = NULL) + : Setting(description, type, permissions, grblName, name, checker) // There are no GRBL IP settings. + , _defaultValue(defVal) + , _currentValue(defVal) +{ } + +IPaddrSetting::IPaddrSetting(const char *description, type_t type, permissions_t permissions, const char * grblName, const char* name, const char *defVal, bool (*checker)(char *) = NULL) + : Setting(description, type, permissions, grblName, name, checker) +{ + IPAddress ipaddr; + if (ipaddr.fromString(defVal)) { + _defaultValue = ipaddr; + _currentValue = _defaultValue; + } else { + throw std::runtime_error("Bad IPaddr default"); + } +} + +void IPaddrSetting::load() { + esp_err_t err = nvs_get_i32(_handle, _keyName, (int32_t *)&_storedValue); + if (err) { + _storedValue = 0x000000ff; // Unreasonable value for any IP thing + _currentValue = _defaultValue; + } else { + _currentValue = _storedValue; + } +} + +void IPaddrSetting::setDefault() { + _currentValue = _defaultValue; + if (_storedValue != _currentValue) { + nvs_erase_key(_handle, _keyName); + } +} + +err_t IPaddrSetting::setStringValue(char* s) { + s = trim(s); + if (err_t err = check(s)) { + return err; + } + IPAddress ipaddr; + if (!ipaddr.fromString(s)) { + return STATUS_INVALID_VALUE; + } + _currentValue = ipaddr; + if (_storedValue != _currentValue) { + if (_currentValue == _defaultValue) { + nvs_erase_key(_handle, _keyName); + } else { + if (nvs_set_i32(_handle, _keyName, (int32_t)_currentValue)) { + return STATUS_NVS_SET_FAILED; + } + _storedValue = _currentValue; + } + } + return STATUS_OK; +} + +const char* IPaddrSetting::getStringValue() { + static String s; + IPAddress ipaddr(get()); + s = ipaddr.toString(); + return s.c_str(); +} + +void IPaddrSetting::addWebui(JSONencoder *j) { + if (getDescription()) { + j->begin_webui(getName(), getDescription(), "A", getStringValue()); + j->end_object(); + } +} + + AxisSettings::AxisSettings(const char *axisName) : + name(axisName) +{} + +err_t GrblCommand::action(char* value, auth_t auth_type, ESPResponseStream* out) { + if (sys.state & _disallowedStates) { + return STATUS_IDLE_ERROR; + } + return _action((const char*)value, auth_type, out); +}; diff --git a/Grbl_Esp32/SettingsClass.h b/Grbl_Esp32/SettingsClass.h new file mode 100644 index 00000000..a7b2c819 --- /dev/null +++ b/Grbl_Esp32/SettingsClass.h @@ -0,0 +1,351 @@ +#pragma once +#include "JSONencoder.h" +#include +#include +#include "espresponse.h" + +// Command::List is a linked list of all settings, +// so common code can enumerate them. +class Command; +// extern Command *CommandsList; + +// This abstract class defines the generic interface that +// is used to set and get values for all settings independent +// of their underlying data type. The values are always +// represented as human-readable strings. This generic +// interface is used for managing settings via the user interface. + +// Derived classes implement these generic functions for different +// kinds of data. Code that accesses settings should use only these +// generic functions and should not use derived classes directly. + +enum { + NO_AXIS = 255, +}; +typedef enum : uint8_t { + GRBL = 1, // Classic GRBL settings like $100 + EXTENDED, // Settings added by early versions of Grbl_Esp32 + WEBSET, // Settings for ESP3D_WebUI, stored in NVS + GRBLCMD, // Non-persistent GRBL commands like $H + WEBCMD, // ESP3D_WebUI commands that are not directly settings +} type_t; +typedef enum : uint8_t { + WG, // Readable and writable as guest + WU, // Readable and writable as user and admin + WA, // Readable as user and admin, writable as admin +} permissions_t; +typedef uint8_t axis_t; + +class Word { +protected: + const char* _description; + const char* _grblName; + const char* _fullName; + type_t _type; + permissions_t _permissions; +public: + Word(type_t type, permissions_t permissions, const char *description, const char * grblName, const char* fullName); + type_t getType() { return _type; } + permissions_t getPermissions() { return _permissions; } + const char* getName() { return _fullName; } + const char* getGrblName() { return _grblName; } + const char* getDescription() { return _description; } +}; + +class Command : public Word { +protected: + Command *link; // linked list of setting objects +public: + static Command* List; + Command* next() { return link; } + + ~Command() {} + Command(const char *description, type_t type, permissions_t permissions, const char * grblName, const char* fullName); + + // The default implementation of addWebui() does nothing. + // Derived classes may override it to do something. + virtual void addWebui(JSONencoder *) {}; + + virtual err_t action(char* value, auth_t auth_level, ESPResponseStream* out) =0; +}; + +class Setting : public Word { +private: +protected: + static nvs_handle _handle; + // group_t _group; + axis_t _axis = NO_AXIS; + Setting *link; // linked list of setting objects + + bool (*_checker)(char *); + const char* _keyName; +public: + static void init(); + static Setting* List; + Setting* next() { return link; } + + err_t check(char *s); + + static err_t report_nvs_stats(const char* value, auth_t auth_level, ESPResponseStream* out) { + nvs_stats_t stats; + if (err_t err = nvs_get_stats(NULL, &stats)) + return err; + grbl_sendf(out->client(), "[MSG: NVS Used: %d Free: %d Total: %d]\r\n", + stats.used_entries, stats.free_entries, stats.total_entries); +#if 0 // The SDK we use does not have this yet + nvs_iterator_t it = nvs_entry_find(NULL, NULL, NVS_TYPE_ANY); + while (it != NULL) { + nvs_entry_info_t info; + nvs_entry_info(it, &info); + it = nvs_entry_next(it); + grbl_sendf(out->client(), "namespace %s key '%s', type '%d' \n", info.namespace_name, info.key, info.type); + } +#endif + return STATUS_OK; + } + + static err_t eraseNVS(const char* value, auth_t auth_level, ESPResponseStream* out) { + nvs_erase_all(_handle); + // return STATUS_OK; + return 0; + } + + ~Setting() {} + // Setting(const char *description, group_t group, const char * grblName, const char* fullName, bool (*checker)(char *)); + Setting(const char *description, type_t type, permissions_t permissions, const char * grblName, const char* fullName, bool (*checker)(char *)); + axis_t getAxis() { return _axis; } + void setAxis(axis_t axis) { _axis = axis; } + + // load() reads the backing store to get the current + // value of the setting. This could be slow so it + // should be done infrequently, typically once at startup. + virtual void load() {}; + virtual void setDefault() {}; + + // The default implementation of addWebui() does nothing. + // Derived classes may override it to do something. + virtual void addWebui(JSONencoder *) {}; + + virtual err_t setStringValue(char* value) =0; + err_t setStringValue(String s) { return setStringValue(s.c_str()); } + virtual const char* getStringValue() =0; + virtual const char* getCompatibleValue() { return getStringValue(); } +}; + +class IntSetting : public Setting { +private: + int32_t _defaultValue; + int32_t _currentValue; + int32_t _storedValue; + int32_t _minValue; + int32_t _maxValue; + +public: + IntSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, int32_t defVal, int32_t minVal, int32_t maxVal, bool (*checker)(char *)); + + IntSetting(type_t type, permissions_t permissions, const char* grblName, const char* name, int32_t defVal, int32_t minVal, int32_t maxVal, bool (*checker)(char *) = NULL) + : IntSetting(NULL, type, permissions, grblName, name, defVal, minVal, maxVal, checker) + { } + + void load(); + void setDefault(); + void addWebui(JSONencoder *); + err_t setStringValue(char* value); + const char* getStringValue(); + + int32_t get() { return _currentValue; } +}; + +class AxisMaskSetting : public Setting { +private: + int32_t _defaultValue; + int32_t _currentValue; + int32_t _storedValue; + +public: + AxisMaskSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, int32_t defVal, bool (*checker)(char *)); + + AxisMaskSetting(type_t type, permissions_t permissions, const char* grblName, const char* name, int32_t defVal, bool (*checker)(char *) = NULL) + : AxisMaskSetting(NULL, type, permissions, grblName, name, defVal, checker) + { } + + void load(); + void setDefault(); + void addWebui(JSONencoder *); + err_t setStringValue(char* value); + const char* getCompatibleValue(); + const char* getStringValue(); + + int32_t get() { return _currentValue; } +}; + +class FloatSetting : public Setting { +private: + float _defaultValue; + float _currentValue; + float _storedValue; + float _minValue; + float _maxValue; +public: + FloatSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, float defVal, float minVal, float maxVal, bool (*checker)(char *)); + + FloatSetting(type_t type, permissions_t permissions, const char* grblName, const char* name, float defVal, float minVal, float maxVal, bool (*checker)(char *) = NULL) + : FloatSetting(NULL, type, permissions, grblName, name, defVal, minVal, maxVal, checker) + { } + + void load(); + void setDefault(); + // There are no Float settings in WebUI + void addWebui(JSONencoder *) {} + err_t setStringValue(char* value); + const char* getStringValue(); + + float get() { return _currentValue; } +}; + +#define MAX_SETTING_STRING 256 +class StringSetting : public Setting { +private: + String _defaultValue; + String _currentValue; + String _storedValue; + int _minLength; + int _maxLength; + void _setStoredValue(const char *s); +public: + StringSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, const char* defVal, int min, int max, bool (*checker)(char *)); + + StringSetting(type_t type, permissions_t permissions, const char* grblName, const char* name, const char* defVal, bool (*checker)(char *) = NULL) + : StringSetting(NULL, type, permissions, grblName, name, defVal, 0, 0, checker) + { }; + + void load(); + void setDefault(); + void addWebui(JSONencoder *); + err_t setStringValue(char* value); + const char* getStringValue(); + + const char* get() { return _currentValue.c_str(); } +}; +struct cmp_str +{ + bool operator()(char const *a, char const *b) const + { + return strcasecmp(a, b) < 0; + } +}; +typedef std::map enum_opt_t; + +class EnumSetting : public Setting { +private: + int8_t _defaultValue; + int8_t _storedValue; + int8_t _currentValue; + std::map* _options; +public: + EnumSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, int8_t defVal, enum_opt_t* opts); + + 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) + { } + + void load(); + void setDefault(); + void addWebui(JSONencoder *); + err_t setStringValue(char* value); + const char* getStringValue(); + + int8_t get() { return _currentValue; } +}; + +class FlagSetting : public Setting { +private: + bool _defaultValue; + int8_t _storedValue; + bool _currentValue; +public: + FlagSetting(const char *description, type_t type, permissions_t permissions, const char* grblName, const char* name, bool defVal, bool (*checker)(char *)); + FlagSetting(type_t type, permissions_t permissions, const char* grblName, const char* name, bool defVal, bool (*checker)(char *) = NULL) + : FlagSetting(NULL, type, permissions, grblName, name, defVal, checker) + { } + + void load(); + void setDefault(); + // There are no Flag settings in WebUI + // The booleans are expressed as Enums + void addWebui(JSONencoder *) {} + err_t setStringValue(char* value); + const char* getCompatibleValue(); + const char* getStringValue(); + + bool get() { return _currentValue; } +}; + +class IPaddrSetting : public Setting { +private: + uint32_t _defaultValue; + uint32_t _currentValue; + uint32_t _storedValue; + +public: + IPaddrSetting(const char *description, type_t type, permissions_t permissions, const char * grblName, const char* name, uint32_t defVal, bool (*checker)(char *)); + IPaddrSetting(const char *description, type_t type, permissions_t permissions, const char * grblName, const char* name, const char *defVal, bool (*checker)(char *)); + + void load(); + void setDefault(); + void addWebui(JSONencoder *); + err_t setStringValue(char* value); + const char* getStringValue(); + + uint32_t get() { return _currentValue; } +}; + +class AxisSettings { +public: + const char* name; + FloatSetting *steps_per_mm; + FloatSetting *max_rate; + FloatSetting *acceleration; + FloatSetting *max_travel; + FloatSetting *run_current; + FloatSetting *hold_current; + IntSetting *microsteps; + IntSetting *stallguard; + + AxisSettings(const char *axisName); +}; +class WebCommand : public Command { + private: + err_t (*_action)(char *, auth_t); + const char* password; + public: + WebCommand(const char* description, type_t type, permissions_t permissions, const char * grblName, const char* name, err_t (*action)(char *, auth_t)) : + Command(description, type, permissions, grblName, name), + _action(action) + {} + err_t action(char* value, auth_t auth_level, ESPResponseStream* response); +}; + +enum : uint8_t { + ANY_STATE = 0, + IDLE_OR_ALARM = 0xff & ~STATE_ALARM, + IDLE_OR_JOG = 0xff & ~STATE_JOG, + NOT_CYCLE_OR_HOLD = STATE_CYCLE | STATE_HOLD, +}; + +class GrblCommand : public Command { + private: + err_t (*_action)(const char *, auth_t, ESPResponseStream*); + uint8_t _disallowedStates; + public: + GrblCommand(const char * grblName, const char* name, err_t (*action)(const char*, auth_t, ESPResponseStream*), uint8_t disallowedStates, permissions_t auth) + : Command(NULL, GRBLCMD, auth, grblName, name) + , _action(action) + , _disallowedStates(disallowedStates) + {} + + GrblCommand(const char * grblName, const char* name, err_t (*action)(const char*, auth_t, ESPResponseStream*), uint8_t disallowedStates) + : GrblCommand(grblName, name, action, disallowedStates, WG) + {} + err_t action(char* value, auth_t auth_level, ESPResponseStream* response); +}; diff --git a/Grbl_Esp32/SettingsDefinitions.cpp b/Grbl_Esp32/SettingsDefinitions.cpp new file mode 100644 index 00000000..d7a0f7a7 --- /dev/null +++ b/Grbl_Esp32/SettingsDefinitions.cpp @@ -0,0 +1,316 @@ +#include "grbl.h" + +bool motorSettingChanged = false; + +StringSetting* startup_line_0; +StringSetting* startup_line_1; +StringSetting* build_info; + +IntSetting* pulse_microseconds; +IntSetting* stepper_idle_lock_time; + +AxisMaskSetting* step_invert_mask; +AxisMaskSetting* dir_invert_mask; +// TODO Settings - need to call st_generate_step_invert_masks; +AxisMaskSetting* homing_dir_mask; +AxisMaskSetting* stallguard_debug_mask; + +FlagSetting* step_enable_invert; +FlagSetting* limit_invert; +FlagSetting* probe_invert; +FlagSetting* report_inches; +FlagSetting* soft_limits; +// TODO Settings - need to check for HOMING_ENABLE +FlagSetting* hard_limits; +// TODO Settings - need to call limits_init; +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* status_mask; +FloatSetting* junction_deviation; +FloatSetting* arc_tolerance; + +FloatSetting* homing_feed_rate; +FloatSetting* homing_seek_rate; +FloatSetting* homing_debounce; +FloatSetting* homing_pulloff; +FloatSetting* spindle_pwm_freq; +FloatSetting* rpm_max; +FloatSetting* rpm_min; + +FloatSetting* spindle_pwm_off_value; +FloatSetting* spindle_pwm_min_value; +FloatSetting* spindle_pwm_max_value; +IntSetting* spindle_pwm_bit_precision; + +EnumSetting* spindle_type; + +enum_opt_t spindleTypes = { + { "NONE", SPINDLE_TYPE_NONE, }, + { "PWM", SPINDLE_TYPE_PWM, }, + { "RELAY", SPINDLE_TYPE_RELAY, }, + { "LASER", SPINDLE_TYPE_LASER, }, + { "DAC", SPINDLE_TYPE_DAC, }, + { "HUANYANG", SPINDLE_TYPE_HUANYANG, }, + { "BESC", SPINDLE_TYPE_BESC, }, + { "10V", SPINDLE_TYPE_10V, }, +}; + +AxisSettings* x_axis_settings; +AxisSettings* y_axis_settings; +AxisSettings* z_axis_settings; +AxisSettings* a_axis_settings; +AxisSettings* b_axis_settings; +AxisSettings* c_axis_settings; + +AxisSettings* axis_settings[MAX_N_AXIS]; + +typedef struct { + const char* name; + float steps_per_mm; + float max_rate; + float acceleration; + float max_travel; + float run_current; + float hold_current; + uint16_t microsteps; + uint16_t stallguard; +} axis_defaults_t; +axis_defaults_t axis_defaults[] = { + { + "X", + DEFAULT_X_STEPS_PER_MM, + DEFAULT_X_MAX_RATE, + DEFAULT_X_ACCELERATION, + DEFAULT_X_MAX_TRAVEL, + DEFAULT_X_CURRENT, + DEFAULT_X_HOLD_CURRENT, + DEFAULT_X_MICROSTEPS, + DEFAULT_X_STALLGUARD + }, + { + "Y", + DEFAULT_Y_STEPS_PER_MM, + DEFAULT_Y_MAX_RATE, + DEFAULT_Y_ACCELERATION, + DEFAULT_Y_MAX_TRAVEL, + DEFAULT_Y_CURRENT, + DEFAULT_Y_HOLD_CURRENT, + DEFAULT_Y_MICROSTEPS, + DEFAULT_Y_STALLGUARD + }, + { + "Z", + DEFAULT_Z_STEPS_PER_MM, + DEFAULT_Z_MAX_RATE, + DEFAULT_Z_ACCELERATION, + DEFAULT_Z_MAX_TRAVEL, + DEFAULT_Z_CURRENT, + DEFAULT_Z_HOLD_CURRENT, + DEFAULT_Z_MICROSTEPS, + DEFAULT_Z_STALLGUARD + }, + { + "A", + DEFAULT_A_STEPS_PER_MM, + DEFAULT_A_MAX_RATE, + DEFAULT_A_ACCELERATION, + DEFAULT_A_MAX_TRAVEL, + DEFAULT_A_CURRENT, + DEFAULT_A_HOLD_CURRENT, + DEFAULT_A_MICROSTEPS, + DEFAULT_A_STALLGUARD + }, + { + "B", + DEFAULT_B_STEPS_PER_MM, + DEFAULT_B_MAX_RATE, + DEFAULT_B_ACCELERATION, + DEFAULT_B_MAX_TRAVEL, + DEFAULT_B_CURRENT, + DEFAULT_B_HOLD_CURRENT, + DEFAULT_B_MICROSTEPS, + DEFAULT_B_STALLGUARD + }, + { + "C", + DEFAULT_C_STEPS_PER_MM, + DEFAULT_C_MAX_RATE, + DEFAULT_C_ACCELERATION, + DEFAULT_C_MAX_TRAVEL, + DEFAULT_C_CURRENT, + DEFAULT_C_HOLD_CURRENT, + DEFAULT_C_MICROSTEPS, + DEFAULT_C_STALLGUARD + } +}; + +// Construct e.g. X_MAX_RATE from axisName "X" and tail "_MAX_RATE" +// in dynamically allocated memory that will not be freed. + +static const char *makename(const char *axisName, const char *tail) { + char* retval = (char *)malloc(strlen(axisName) + strlen(tail) + 2); + + strcpy(retval, axisName); + strcat(retval, "/"); + return strcat(retval, tail); +} + +static bool checkStartupLine(char* value) { + if (sys.state != STATE_IDLE) + return STATUS_IDLE_ERROR; + return gc_execute_line(value, CLIENT_SERIAL) == 0; +} + +static bool checkStallguard(char* value) { + 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; + return true; +} + +// Generates a string like "122" from axisNum 2 and base 120 +static const char* makeGrblName(int axisNum, int base) { + // To omit A,B,C axes: + // if (axisNum > 2) return NULL; + char buf[4]; + snprintf(buf, 4, "%d", axisNum + base); + char* retval = (char*)malloc(strlen(buf)); + return strcpy(retval, buf); +} + +void make_settings() { + Setting::init(); + + // Create the axis settings in the order that people are + // accustomed to seeing. + int axis; + axis_defaults_t* def; + for (axis = 0; axis < N_AXIS; axis++) { + def = &axis_defaults[axis]; + axis_settings[axis] = new AxisSettings(def->name); + } + x_axis_settings = axis_settings[X_AXIS]; + y_axis_settings = axis_settings[Y_AXIS]; + z_axis_settings = axis_settings[Z_AXIS]; + a_axis_settings = axis_settings[A_AXIS]; + b_axis_settings = axis_settings[B_AXIS]; + c_axis_settings = axis_settings[C_AXIS]; + for (axis = 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); + setting->setAxis(axis); + axis_settings[axis]->stallguard = setting; + } + for (axis = 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); + setting->setAxis(axis); + axis_settings[axis]->microsteps = setting; + } + for (axis = 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 + setting->setAxis(axis); + axis_settings[axis]->hold_current = setting; + } + for (axis = 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 + setting->setAxis(axis); + axis_settings[axis]->run_current = setting; + } + for (axis = N_AXIS - 1; axis >= 0; axis--) { + def = &axis_defaults[axis]; + auto setting = new FloatSetting(GRBL, WG, makeGrblName(axis, 130), makename(def->name, "MaxTravel"), def->max_travel, 1.0, 100000.0); + setting->setAxis(axis); + axis_settings[axis]->max_travel = setting; + } + for (axis = N_AXIS - 1; axis >= 0; axis--) { + def = &axis_defaults[axis]; + auto setting = new FloatSetting(GRBL, WG, makeGrblName(axis, 120), makename(def->name, "Acceleration"), def->acceleration, 1.0, 100000.0); + setting->setAxis(axis); + axis_settings[axis]->acceleration = setting; + } + for (axis = N_AXIS - 1; axis >= 0; axis--) { + def = &axis_defaults[axis]; + auto setting = new FloatSetting(GRBL, WG, makeGrblName(axis, 110), makename(def->name, "MaxRate"), def->max_rate, 1.0, 100000.0); + setting->setAxis(axis); + axis_settings[axis]->max_rate = setting; + } + for (axis = N_AXIS - 1; axis >= 0; axis--) { + def = &axis_defaults[axis]; + auto setting = new FloatSetting(GRBL, WG, makeGrblName(axis, 100), makename(def->name, "StepsPerMm"), def->steps_per_mm, 1.0, 100000.0); + setting->setAxis(axis); + axis_settings[axis]->steps_per_mm = setting; + } + + // 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 + // 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); + + // 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); + // 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); + + + 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); + homing_seek_rate = new FloatSetting(GRBL, WG, "25", "Homing/Seek", DEFAULT_HOMING_SEEK_RATE, 0, 10000); + homing_feed_rate = new FloatSetting(GRBL, WG, "24", "Homing/Feed", DEFAULT_HOMING_FEED_RATE, 0, 10000); + + // TODO Settings - need to call st_generate_step_invert_masks() + homing_dir_mask = new AxisMaskSetting(GRBL, WG, "23", "Homing/DirInvert", DEFAULT_HOMING_DIR_MASK); + + // TODO Settings - need to call limits_init(); + homing_enable = new FlagSetting(GRBL, WG, "22", "Homing/Enable", DEFAULT_HOMING_ENABLE); + // TODO Settings - need to check for HOMING_ENABLE + hard_limits = new FlagSetting(GRBL, WG, "21", "Limits/Hard", DEFAULT_HARD_LIMIT_ENABLE); + soft_limits = new FlagSetting(GRBL, WG, "20", "Limits/Soft", DEFAULT_SOFT_LIMIT_ENABLE, NULL); + + report_inches = new FlagSetting(GRBL, WG, "13", "Report/Inches", DEFAULT_REPORT_INCHES); + // TODO Settings - also need to clear, but not set, soft_limits + arc_tolerance = new FloatSetting(GRBL, WG, "12", "GCode/ArcTolerance", DEFAULT_ARC_TOLERANCE, 0, 1); + junction_deviation = new FloatSetting(GRBL, WG, "11", "GCode/JunctionDeviation", DEFAULT_JUNCTION_DEVIATION, 0, 10); + status_mask = new IntSetting(GRBL, WG, "10", "Report/Status", DEFAULT_STATUS_REPORT_MASK, 0, 2); + + probe_invert = new FlagSetting(GRBL, WG, "6", "Probe/Invert", DEFAULT_INVERT_PROBE_PIN); + limit_invert = new FlagSetting(GRBL, WG, "5", "Limits/Invert", DEFAULT_INVERT_LIMIT_PINS); + step_enable_invert = new FlagSetting(GRBL, WG, "4", "Stepper/EnableInvert", DEFAULT_INVERT_ST_ENABLE); + dir_invert_mask = new AxisMaskSetting(GRBL, WG, "3", "Stepper/DirInvert", DEFAULT_DIRECTION_INVERT_MASK); + step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK); + 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", SPINDLE_TYPE_NONE, &spindleTypes); + stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, checkStallguardDebugMask); +} diff --git a/Grbl_Esp32/SettingsDefinitions.h b/Grbl_Esp32/SettingsDefinitions.h new file mode 100644 index 00000000..a87ac054 --- /dev/null +++ b/Grbl_Esp32/SettingsDefinitions.h @@ -0,0 +1,51 @@ +#pragma once +extern bool motorSettingChanged; + +extern AxisSettings* x_axis_settings; +extern AxisSettings* y_axis_settings; +extern AxisSettings* z_axis_settings; +extern AxisSettings* a_axis_settings; +extern AxisSettings* b_axis_settings; +extern AxisSettings* c_axis_settings; +extern AxisSettings* axis_settings[]; + +extern StringSetting* startup_line_0; +extern StringSetting* startup_line_1; +extern StringSetting* build_info; + +extern IntSetting* pulse_microseconds; +extern IntSetting* stepper_idle_lock_time; + +extern AxisMaskSetting* step_invert_mask; +extern AxisMaskSetting* dir_invert_mask; +extern AxisMaskSetting* homing_dir_mask; + +extern FlagSetting* step_enable_invert; +extern FlagSetting* limit_invert; +extern FlagSetting* probe_invert; +extern FlagSetting* report_inches; +extern FlagSetting* soft_limits; +extern FlagSetting* hard_limits; +extern FlagSetting* homing_enable; +extern FlagSetting* laser_mode; + +extern IntSetting* status_mask; +extern FloatSetting* junction_deviation; +extern FloatSetting* arc_tolerance; + +extern FloatSetting* homing_feed_rate; +extern FloatSetting* homing_seek_rate; +extern FloatSetting* homing_debounce; +extern FloatSetting* homing_pulloff; +extern FloatSetting* spindle_pwm_freq; +extern FloatSetting* rpm_max; +extern FloatSetting* rpm_min; + +extern FloatSetting* spindle_pwm_off_value; +extern FloatSetting* spindle_pwm_min_value; +extern FloatSetting* spindle_pwm_max_value; +extern IntSetting* spindle_pwm_bit_precision; + +extern EnumSetting* spindle_type; + +extern AxisMaskSetting* stallguard_debug_mask; diff --git a/Grbl_Esp32/Spindles/10vSpindle.cpp b/Grbl_Esp32/Spindles/10vSpindle.cpp new file mode 100644 index 00000000..e6a37629 --- /dev/null +++ b/Grbl_Esp32/Spindles/10vSpindle.cpp @@ -0,0 +1,167 @@ +/* + 10vSpindle.cpp + + This is basically a PWM spindle with some changes, so a forward and + reverse signal can be sent. + + The direction pins will act as enables for the 2 directions. There is usually + a min RPM with VFDs, that speed will remain even if speed is 0. You + must turn off both direction pins when enable is off. + + + Part of Grbl_ESP32 + 2020 - Bart Dring + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . + + + +*/ +#include "SpindleClass.h" + + + +void _10vSpindle :: init() { + + get_pins_and_settings(); // these gets the standard PWM settings, but many need to be changed for BESC + + // a couple more pins not inherited from PWM Spindle + #ifdef SPINDLE_FORWARD_PIN + _forward_pin = SPINDLE_FORWARD_PIN; + #else + _forward_pin = UNDEFINED_PIN; + #endif + + #ifdef SPINDLE_REVERSE_PIN + _reverse_pin = SPINDLE_REVERSE_PIN; + #else + _reverse_pin = UNDEFINED_PIN; + #endif + + if (_output_pin == UNDEFINED_PIN) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning: BESC output pin not defined"); + return; // We cannot continue without the output pin + } + + ledcSetup(_spindle_pwm_chan_num, (double)_pwm_freq, _pwm_precision); // setup the channel + ledcAttachPin(_output_pin, _spindle_pwm_chan_num); // attach the PWM to the pin + + pinMode(_enable_pin, OUTPUT); + pinMode(_direction_pin, OUTPUT); + pinMode(_forward_pin, OUTPUT); + pinMode(_reverse_pin, OUTPUT); + + set_rpm(0); + + config_message(); + + is_reversable = true; // these VFDs are always reversable +} + +// prints the startup message of the spindle config +void _10vSpindle :: config_message() { + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "0-10V spindle Out:%s Enbl:%s, Dir:%s, Fwd:%s, Rev:%s, Freq:%dHz Res:%dbits", + pinName(_output_pin).c_str(), + pinName(_enable_pin).c_str(), + pinName(_direction_pin).c_str(), + pinName(_forward_pin).c_str(), + pinName(_reverse_pin).c_str(), + _pwm_freq, + _pwm_precision); +} + +uint32_t _10vSpindle::set_rpm(uint32_t rpm) { + uint32_t pwm_value; + + if (_output_pin == UNDEFINED_PIN) + return rpm; + + // apply speed overrides + rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (percent) + + // apply limits limits + if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) + rpm = _max_rpm; + else if (rpm != 0 && rpm <= _min_rpm) + rpm = _min_rpm; + sys.spindle_speed = rpm; + + // determine the pwm value + if (rpm == 0) { + pwm_value = _pwm_off_value; + } else { + pwm_value = map_uint32_t(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value); + } + + set_output(pwm_value); + return rpm; +} + +void _10vSpindle::set_state(uint8_t state, uint32_t rpm) { + if (sys.abort) + return; // Block during abort. + + if (state == SPINDLE_DISABLE) { // Halt or set spindle direction and rpm. + sys.spindle_speed = 0; + stop(); + } else { + set_spindle_dir_pin(state == SPINDLE_ENABLE_CW); + set_rpm(rpm); + } + + set_enable_pin(state != SPINDLE_DISABLE); + + sys.report_ovr_counter = 0; // Set to report change immediately +} + +uint8_t _10vSpindle::get_state() { + if (_current_pwm_duty == 0 || _output_pin == UNDEFINED_PIN) + return (SPINDLE_STATE_DISABLE); + if (_direction_pin != UNDEFINED_PIN) + return digitalRead(_direction_pin) ? SPINDLE_STATE_CW : SPINDLE_STATE_CCW; + return (SPINDLE_STATE_CW); +} + +void _10vSpindle::stop() { + // inverts are delt with in methods + set_enable_pin(false); + set_output(_pwm_off_value); +} + +void _10vSpindle::set_enable_pin(bool enable) { +grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "_10vSpindle::set_enable_pin"); +if (_off_with_zero_speed && sys.spindle_speed == 0) + enable = false; + +#ifdef INVERT_SPINDLE_ENABLE_PIN + enable = !enable; +#endif + digitalWrite(_enable_pin, enable); + + // turn off anything that acts like an enable + if (!enable) { + digitalWrite(_direction_pin, enable); + digitalWrite(_forward_pin, enable); + digitalWrite(_reverse_pin, enable); + } + + +} + +void _10vSpindle::set_spindle_dir_pin(bool Clockwise) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "_10vSpindle::set_spindle_dir_pin"); + digitalWrite(_direction_pin, Clockwise); + digitalWrite(_forward_pin, Clockwise); + digitalWrite(_reverse_pin, ! Clockwise); +} diff --git a/Grbl_Esp32/Spindles/BESCSpindle.cpp b/Grbl_Esp32/Spindles/BESCSpindle.cpp index b6fccf71..32b5e145 100644 --- a/Grbl_Esp32/Spindles/BESCSpindle.cpp +++ b/Grbl_Esp32/Spindles/BESCSpindle.cpp @@ -73,8 +73,7 @@ void BESCSpindle :: init() { ledcSetup(_spindle_pwm_chan_num, (double)_pwm_freq, _pwm_precision); // setup the channel ledcAttachPin(_output_pin, _spindle_pwm_chan_num); // attach the PWM to the pin - if (_enable_pin != UNDEFINED_PIN) - pinMode(_enable_pin, OUTPUT); + pinMode(_enable_pin, OUTPUT); set_rpm(0); @@ -85,12 +84,12 @@ void BESCSpindle :: init() { void BESCSpindle :: config_message() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, - "BESC spindle on Pin:%d Min:%0.2fms Max:%0.2fms Freq:%dHz Res:%dbits", - report_pin_number(_output_pin), + "BESC spindle on Pin:%s Min:%0.2fms Max:%0.2fms Freq:%dHz Res:%dbits", + pinName(_output_pin).c_str(), BESC_MIN_PULSE_SECS * 1000.0, // convert to milliseconds BESC_MAX_PULSE_SECS * 1000.0, // convert to milliseconds - report_pin_number(_pwm_freq), - report_pin_number(_pwm_precision)); + _pwm_freq, + _pwm_precision); } uint32_t BESCSpindle::set_rpm(uint32_t rpm) { @@ -108,7 +107,7 @@ uint32_t BESCSpindle::set_rpm(uint32_t rpm) { else if (rpm != 0 && rpm <= _min_rpm) rpm = _min_rpm; sys.spindle_speed = rpm; - + // determine the pwm value if (rpm == 0) { pwm_value = _pwm_off_value; diff --git a/Grbl_Esp32/Spindles/DacSpindle.cpp b/Grbl_Esp32/Spindles/DacSpindle.cpp index 0e51de67..952ad6b4 100644 --- a/Grbl_Esp32/Spindles/DacSpindle.cpp +++ b/Grbl_Esp32/Spindles/DacSpindle.cpp @@ -30,8 +30,8 @@ void DacSpindle :: init() { if (_output_pin == UNDEFINED_PIN) return; - _min_rpm = settings.rpm_min; - _max_rpm = settings.rpm_max; + _min_rpm = rpm_min->get(); + _max_rpm = rpm_max->get(); _pwm_min_value = 0; // not actually PWM...DAC counts _pwm_max_value = 255; // not actually PWM...DAC counts _gpio_ok = true; @@ -42,12 +42,8 @@ void DacSpindle :: init() { return; } - if (_enable_pin != UNDEFINED_PIN) - pinMode(_enable_pin, OUTPUT); - - if (_direction_pin != UNDEFINED_PIN) { - pinMode(_direction_pin, OUTPUT); - } + pinMode(_enable_pin, OUTPUT); + pinMode(_direction_pin, OUTPUT); is_reversable = (_direction_pin != UNDEFINED_PIN); @@ -57,17 +53,17 @@ void DacSpindle :: init() { void DacSpindle :: config_message() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, - "DAC spindle Output:%d, Enbl:%d, Dir:%d, Res:8bits", - report_pin_number(_output_pin), - report_pin_number(_enable_pin), // 255 means pin not defined - report_pin_number(_direction_pin)); // 255 means pin not defined + "DAC spindle Output:%s, Enbl:%s, Dir:%s, Res:8bits", + pinName(_output_pin).c_str(), + pinName(_enable_pin).c_str(), + pinName(_direction_pin).c_str()); } uint32_t DacSpindle::set_rpm(uint32_t rpm) { if (_output_pin == UNDEFINED_PIN) return rpm; - uint32_t pwm_value; + uint32_t pwm_value; // apply overrides and limits rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (percent) diff --git a/Grbl_Esp32/Spindles/HuanyangSpindle.cpp b/Grbl_Esp32/Spindles/HuanyangSpindle.cpp index f25b65f1..1183fab1 100644 --- a/Grbl_Esp32/Spindles/HuanyangSpindle.cpp +++ b/Grbl_Esp32/Spindles/HuanyangSpindle.cpp @@ -116,7 +116,7 @@ void HuanyangSpindle :: init() { 1, // priority &vfd_cmdTaskHandle, 0 // core - ); + ); _task_running = true; } @@ -196,12 +196,12 @@ bool HuanyangSpindle :: get_pins_and_settings() { } void HuanyangSpindle :: config_message() { - grbl_msg_sendf(CLIENT_SERIAL, - MSG_LEVEL_INFO, - "Huanyang Spindle Tx:%d Rx:%d RTS:%d", - report_pin_number(_txd_pin), - report_pin_number(_rxd_pin), - report_pin_number(_rts_pin)); + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "Huanyang Spindle Tx:%s Rx:%s RTS:%s", + pinName(_txd_pin).c_str(), + pinName(_rxd_pin).c_str(), + pinName(_rts_pin).c_str()); } /* @@ -211,8 +211,6 @@ void HuanyangSpindle :: config_message() { 0x01 0x03 0x01 0x11 0x30 0x44 Start spindle counter-clockwise */ void HuanyangSpindle :: set_state(uint8_t state, uint32_t rpm) { - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "VFD Set state"); - if (sys.abort) return; // Block during abort. diff --git a/Grbl_Esp32/Spindles/Laser.cpp b/Grbl_Esp32/Spindles/Laser.cpp index 1eb97561..f092af2a 100644 --- a/Grbl_Esp32/Spindles/Laser.cpp +++ b/Grbl_Esp32/Spindles/Laser.cpp @@ -31,8 +31,8 @@ bool Laser :: isRateAdjusted() { void Laser :: config_message() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, - "Laser spindle on GPIO:%d, Freq:%.2fHz, Res:%dbits Laser mode:$32=%d", - report_pin_number(_output_pin), + "Laser spindle on Pin:%s, Freq:%.2fHz, Res:%dbits Laser mode:$32=%d", + pinName(_output_pin).c_str(), _pwm_freq, _pwm_precision, isRateAdjusted()); // the current mode diff --git a/Grbl_Esp32/Spindles/PWMSpindle.cpp b/Grbl_Esp32/Spindles/PWMSpindle.cpp index e4e053b7..e37d6a95 100644 --- a/Grbl_Esp32/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/Spindles/PWMSpindle.cpp @@ -38,14 +38,17 @@ void PWMSpindle :: init() { return; // We cannot continue without the output pin } + if (_output_pin >= I2S_OUT_PIN_BASE) { + + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning: Spindle output pin %s cannot do PWM", pinName(_output_pin).c_str()); + return; + } + ledcSetup(_spindle_pwm_chan_num, (double)_pwm_freq, _pwm_precision); // setup the channel ledcAttachPin(_output_pin, _spindle_pwm_chan_num); // attach the PWM to the pin - if (_enable_pin != UNDEFINED_PIN) - pinMode(_enable_pin, OUTPUT); - - if (_direction_pin != UNDEFINED_PIN) - pinMode(_direction_pin, OUTPUT); + pinMode(_enable_pin, OUTPUT); + pinMode(_direction_pin, OUTPUT); config_message(); } @@ -60,7 +63,7 @@ void PWMSpindle :: get_pins_and_settings() { _output_pin = UNDEFINED_PIN; #endif -#ifdef INVERT_SPINDLE_ENABLE_PIN +#ifdef INVERT_SPINDLE_OUTPUT_PIN _invert_pwm = true; #else _invert_pwm = false; @@ -85,25 +88,25 @@ void PWMSpindle :: get_pins_and_settings() { is_reversable = (_direction_pin != UNDEFINED_PIN); - _pwm_freq = (uint32_t)settings.spindle_pwm_freq; + _pwm_freq = spindle_pwm_freq->get(); _pwm_precision = calc_pwm_precision(_pwm_freq); // detewrmine the best precision _pwm_period = (1 << _pwm_precision); - if (settings.spindle_pwm_min_value > settings.spindle_pwm_min_value) + if (spindle_pwm_min_value->get() > spindle_pwm_min_value->get()) grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning: Spindle min pwm is greater than max. Check $35 and $36"); // pre-caculate some PWM count values - _pwm_off_value = (_pwm_period * (uint32_t)settings.spindle_pwm_off_value / 100.0); - _pwm_min_value = (_pwm_period * (uint32_t)settings.spindle_pwm_min_value / 100.0); - _pwm_max_value = (_pwm_period * (uint32_t)settings.spindle_pwm_max_value / 100.0); + _pwm_off_value = (_pwm_period * spindle_pwm_off_value->get() / 100.0); + _pwm_min_value = (_pwm_period * spindle_pwm_min_value->get() / 100.0); + _pwm_max_value = (_pwm_period * spindle_pwm_max_value->get() / 100.0); #ifdef ENABLE_PIECEWISE_LINEAR_SPINDLE _min_rpm = RPM_MIN; _max_rpm = RPM_MAX; _piecewide_linear = true; #else - _min_rpm = (uint32_t)settings.rpm_min; - _max_rpm = (uint32_t)settings.rpm_max; + _min_rpm = rpm_min->get(); + _max_rpm = rpm_max->get(); _piecewide_linear = false; #endif // The pwm_gradient is the pwm duty cycle units per rpm @@ -168,19 +171,11 @@ void PWMSpindle::set_state(uint8_t state, uint32_t rpm) { } uint8_t PWMSpindle::get_state() { - - if (_current_pwm_duty == 0 || _output_pin == UNDEFINED_PIN) return (SPINDLE_STATE_DISABLE); - else { - if (_direction_pin != UNDEFINED_PIN) { - if (digitalRead(_direction_pin)) - return (SPINDLE_STATE_CW); - else - return (SPINDLE_STATE_CCW); - } else - return (SPINDLE_STATE_CW); - } + if (_direction_pin != UNDEFINED_PIN) + return digitalRead(_direction_pin) ? SPINDLE_STATE_CW : SPINDLE_STATE_CCW; + return (SPINDLE_STATE_CW); } void PWMSpindle::stop() { @@ -193,10 +188,10 @@ void PWMSpindle::stop() { void PWMSpindle :: config_message() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, - "PWM spindle Output:%d, Enbl:%d, Dir:%d, Freq:%dHz, Res:%dbits", - report_pin_number(_output_pin), - report_pin_number(_enable_pin), // 255 means pin not defined - report_pin_number(_direction_pin), // 255 means pin not defined + "PWM spindle Output:%s, Enbl:%s, Dir:%s, Freq:%dHz, Res:%dbits", + pinName(_output_pin).c_str(), + pinName(_enable_pin).c_str(), + pinName(_direction_pin).c_str(), _pwm_freq, _pwm_precision); } @@ -214,7 +209,7 @@ void PWMSpindle::set_output(uint32_t duty) { _current_pwm_duty = duty; - if (_invert_pwm) + if (_invert_pwm) duty = (1 << _pwm_precision) - duty; ledcWrite(_spindle_pwm_chan_num, duty); @@ -222,6 +217,7 @@ void PWMSpindle::set_output(uint32_t duty) { } void PWMSpindle::set_enable_pin(bool enable) { + if (_enable_pin == UNDEFINED_PIN) return; @@ -233,11 +229,11 @@ void PWMSpindle::set_enable_pin(bool enable) { #else digitalWrite(_enable_pin, !enable); #endif + digitalWrite(_enable_pin, enable); } void PWMSpindle::set_spindle_dir_pin(bool Clockwise) { - if (_direction_pin != UNDEFINED_PIN) - digitalWrite(_direction_pin, Clockwise); + digitalWrite(_direction_pin, Clockwise); } diff --git a/Grbl_Esp32/Spindles/RelaySpindle.cpp b/Grbl_Esp32/Spindles/RelaySpindle.cpp index 76f1444e..dee9cd82 100644 --- a/Grbl_Esp32/Spindles/RelaySpindle.cpp +++ b/Grbl_Esp32/Spindles/RelaySpindle.cpp @@ -32,12 +32,8 @@ void RelaySpindle::init() { return; pinMode(_output_pin, OUTPUT); - - if (_enable_pin != UNDEFINED_PIN) - pinMode(_enable_pin, OUTPUT); - - if (_direction_pin != UNDEFINED_PIN) - pinMode(_direction_pin, OUTPUT); + pinMode(_enable_pin, OUTPUT); + pinMode(_direction_pin, OUTPUT); is_reversable = (_direction_pin != UNDEFINED_PIN); @@ -45,13 +41,13 @@ void RelaySpindle::init() { } // prints the startup message of the spindle config -void RelaySpindle :: config_message() { +void RelaySpindle :: config_message() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, - "Relay spindle Output:%d, Enbl:%d, Dir:%d", - report_pin_number(_output_pin), - report_pin_number(_enable_pin), // 255 means pin not defined - report_pin_number(_direction_pin)); // 255 means pin not defined + "Relay spindle Output:%s, Enbl:%s, Dir:%s", + pinName(_output_pin).c_str(), + pinName(_enable_pin).c_str(), + pinName(_direction_pin).c_str()); } uint32_t RelaySpindle::set_rpm(uint32_t rpm) { @@ -59,12 +55,8 @@ uint32_t RelaySpindle::set_rpm(uint32_t rpm) { return rpm; sys.spindle_speed = rpm; + set_output(rpm != 0); - if (rpm == 0) { - set_output(0); - } else { - set_output(1); - } return rpm; } diff --git a/Grbl_Esp32/Spindles/SpindleClass.cpp b/Grbl_Esp32/Spindles/SpindleClass.cpp index dd148c26..75c3b2ce 100644 --- a/Grbl_Esp32/Spindles/SpindleClass.cpp +++ b/Grbl_Esp32/Spindles/SpindleClass.cpp @@ -34,6 +34,7 @@ #include "Laser.cpp" #include "HuanyangSpindle.cpp" #include "BESCSpindle.cpp" +#include "10vSpindle.cpp" // An instance of each type of spindle is created here. @@ -45,11 +46,12 @@ Laser laser; DacSpindle dac_spindle; HuanyangSpindle huanyang_spindle; BESCSpindle besc_spindle; +_10vSpindle _10v_spindle; -void spindle_select(uint8_t spindle_type) { +void spindle_select() { - switch (spindle_type) { + switch (spindle_type->get()) { case SPINDLE_TYPE_PWM: spindle = &pwm_spindle; break; @@ -68,6 +70,9 @@ void spindle_select(uint8_t spindle_type) { case SPINDLE_TYPE_BESC: spindle = &besc_spindle; break; + case SPINDLE_TYPE_10V: + spindle = &_10v_spindle; + break; case SPINDLE_TYPE_NONE: default: spindle = &null_spindle; diff --git a/Grbl_Esp32/Spindles/SpindleClass.h b/Grbl_Esp32/Spindles/SpindleClass.h index 61db61ad..db86ef23 100644 --- a/Grbl_Esp32/Spindles/SpindleClass.h +++ b/Grbl_Esp32/Spindles/SpindleClass.h @@ -36,6 +36,7 @@ #define SPINDLE_TYPE_DAC 4 #define SPINDLE_TYPE_HUANYANG 5 #define SPINDLE_TYPE_BESC 6 +#define SPINDLE_TYPE_10V 7 #ifndef SPINDLE_CLASS_H #define SPINDLE_CLASS_H @@ -85,12 +86,11 @@ class PWMSpindle : public Spindle { void stop(); void config_message(); - private: - - int32_t _current_pwm_duty; + private: void set_spindle_dir_pin(bool Clockwise); protected: + int32_t _current_pwm_duty; uint32_t _min_rpm; uint32_t _max_rpm; uint32_t _pwm_off_value; @@ -178,6 +178,22 @@ class BESCSpindle : public PWMSpindle { uint32_t set_rpm(uint32_t rpm); }; +class _10vSpindle : public PWMSpindle { + public: + void init(); + void config_message(); + uint32_t set_rpm(uint32_t rpm); + uint8_t _forward_pin; + uint8_t _reverse_pin; + void set_state(uint8_t state, uint32_t rpm); + uint8_t get_state(); + void stop(); + protected: + void set_enable_pin(bool enable_pin); + void set_spindle_dir_pin(bool Clockwise); + +}; + extern Spindle* spindle; @@ -188,8 +204,9 @@ extern Laser laser; extern DacSpindle dac_spindle; extern HuanyangSpindle huanyang_spindle; extern BESCSpindle besc_spindle; +extern _10vSpindle _10v_spindle; -void spindle_select(uint8_t spindletype); +void spindle_select(); // in HuanyangSpindle.cpp void vfd_cmd_task(void* pvParameters); diff --git a/Grbl_Esp32/WebSettings.cpp b/Grbl_Esp32/WebSettings.cpp new file mode 100644 index 00000000..74e79647 --- /dev/null +++ b/Grbl_Esp32/WebSettings.cpp @@ -0,0 +1,1062 @@ +/* + WebSettings.cpp - Settings and Commands for Grbl_ESP32's interface + to ESP3D_WebUI. Code snippets extracted from commands.cpp in the + old WebUI interface code are presented via the Settings class. + + Copyright (c) 2020 Mitch Bradley + Copyright (c) 2014 Luc Lebosse. All rights reserved. + + 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" + +#include +#include +#include +#include +#include + +#include "espresponse.h" +#include "web_server.h" +#include "string.h" + +#ifdef ENABLE_WIFI +StringSetting* wifi_sta_ssid; +StringSetting* wifi_sta_password; + +EnumSetting* wifi_sta_mode; +IPaddrSetting* wifi_sta_ip; +IPaddrSetting* wifi_sta_gateway; +IPaddrSetting* wifi_sta_netmask; + +StringSetting* wifi_ap_ssid; +StringSetting* wifi_ap_password; + +IPaddrSetting* wifi_ap_ip; + +IntSetting* wifi_ap_channel; + +StringSetting* wifi_hostname; +EnumSetting* http_enable; +IntSetting* http_port; +EnumSetting* telnet_enable; +IntSetting* telnet_port; +typedef std::map enum_opt_t; +enum_opt_t staModeOptions = { + { "DHCP", DHCP_MODE , }, + { "Static", STATIC_MODE , }, +}; +#endif + +#ifdef WIFI_OR_BLUETOOTH +EnumSetting* wifi_radio_mode; +enum_opt_t radioOptions = { + { "None", ESP_RADIO_OFF, }, + { "STA", ESP_WIFI_STA, }, + { "AP", ESP_WIFI_AP, }, + { "BT", ESP_BT, }, +}; +enum_opt_t radioEnabledOptions = { + { "NONE", ESP_RADIO_OFF, }, +#ifdef ENABLE_WIFI + { "STA", ESP_WIFI_STA, }, + { "AP", ESP_WIFI_AP, }, +#endif +#ifdef ENABLE_BLUETOOTH + { "BT", ESP_BT, }, +#endif +}; +#endif + +#ifdef ENABLE_BLUETOOTH +StringSetting* bt_name; +#endif + + +#ifdef ENABLE_NOTIFICATIONS +enum_opt_t notificationOptions = { + { "NONE", 0, }, + { "LINE", 3, }, + { "PUSHOVER", 1, }, + { "EMAIL", 2, }, +}; +EnumSetting* notification_type; +StringSetting* notification_t1; +StringSetting* notification_t2; +StringSetting* notification_ts; +#endif + +enum_opt_t onoffOptions = { + { "OFF", 0, }, + { "ON", 1, } +}; + +static ESPResponseStream* espresponse; + +typedef struct { + char* key; + char* value; +} keyval_t; + +static keyval_t params[10]; +bool split_params(char *parameter) { + int i = 0; + for (char* s = parameter; *s; s++) { + if (*s == '=') { + params[i].value = s+1; + *s = '\0'; + // Search backward looking for the start of the key, + // either just after a space or at the beginning of the strin + if (s == parameter) { + return false; + } + for (char* k = s-1; k >= parameter; --k) { + if (*k == '\0') { + // If we find a NUL - i.e. the end of the previous key - + // before finding a space, the string is malformed. + return false; + } + if (*k == ' ') { + *k = '\0'; + params[i++].key = k+1; + break; + } + if (k == parameter) { + params[i++].key = k; + } + } + } + } + params[i].key = NULL; + return true; +} + +char nullstr[1] = { '\0' }; +char* get_param(const char *key, bool allowSpaces) { + for (keyval_t* p = params; p->key; p++) { + if (!strcasecmp(key, p->key)) { + if (!allowSpaces) { + for (char* s = p->value; *s; s++) { + if (*s == ' ') { + *s = '\0'; + break; + } + } + } + return p->value; + } + } + return nullstr; +} + +err_t WebCommand::action(char* value, auth_t auth_level, ESPResponseStream* out) { + char empty = '\0'; + if (!value) { + value = ∅ + } + espresponse = out; + return _action(value, auth_level); +}; + +static int webColumn = 0; +// We create a variety of print functions to make the rest +// of the code more compact and readable. +static void webPrint(const char *s) +{ + if (espresponse) { + espresponse->print(s); + webColumn += strlen(s); + } +} +static void webPrintSetColumn(int column) { + while (webColumn < column) { + webPrint(" "); + } +} +static void webPrint(String s) +{ + webPrint(s.c_str()); +} +static void webPrint(const char *s1, const char *s2) +{ + webPrint(s1); + webPrint(s2); +} +static void webPrint(const char *s1, String s2) +{ + webPrint(s1); + webPrint(s2.c_str()); +} +static void webPrint(const char *s1, const char *s2, const char *s3) +{ + webPrint(s1); + webPrint(s2); + webPrint(s3); +} +static void webPrintln(const char *s) +{ + webPrint(s); + webPrint("\r\n"); + webColumn = 0; +} +static void webPrintln(String s) +{ + webPrintln(s.c_str()); +} +static void webPrintln(const char *s1, const char *s2) +{ + webPrint(s1); + webPrintln(s2); +} +static void webPrintln(const char *s, IPAddress ip) +{ + webPrint(s); + webPrintln(ip.toString().c_str()); +} +static void webPrintln(const char *s, String s2) +{ + webPrintln(s, s2.c_str()); +} + +char *trim(char *str) +{ + char *end; + // Trim leading space + while(isspace((unsigned char)*str)) { + str++; + } + if(*str == 0) { // All spaces? + return str; + } + // Trim trailing space + end = str + strlen(str) - 1; + while(end > str && isspace((unsigned char)*end)) { + end--; + } + // Write new null terminator character + end[1] = '\0'; + return str; +} + +static void print_mac(const char *s, String mac) +{ + webPrint(s); + webPrint(" ("); + webPrint(mac); + webPrintln(")"); +} + +static err_t showFwInfo(char *parameter, auth_t auth_level) { // ESP800 + webPrint("FW version:" GRBL_VERSION " (" GRBL_VERSION_BUILD ")" " # FW target:grbl-embedded # FW HW:"); + #ifdef ENABLE_SD_CARD + webPrint("Direct SD"); + #else + webPrint("No SD"); + #endif + webPrint(" # primary sd:/sd # secondary sd:none # authentication:"); +#ifdef ENABLE_AUTHENTICATION + webPrint("yes"); +#else + webPrint("no"); +#endif +#if defined (ENABLE_WIFI) + #if defined (ENABLE_HTTP) + webPrint(" # webcommunication: Sync: ", String(web_server.port() + 1)); + webPrint(":"); + switch (WiFi.getMode()) { + case WIFI_MODE_AP: + webPrint(WiFi.softAPIP().toString()); + break; + case WIFI_MODE_STA: + webPrint(WiFi.localIP().toString()); + break; + case WIFI_MODE_APSTA: + webPrint(WiFi.softAPIP().toString()); + break; + default: + webPrint("0.0.0.0"); + break; + } + #endif + webPrint(" # hostname:", wifi_config.Hostname()); + if (WiFi.getMode() == WIFI_AP) { + webPrint("(AP mode)"); + } +#endif + //to save time in decoding `?` + webPrintln(" # axis:", String(N_AXIS)); + return STATUS_OK; +} + +static err_t SPIFFSSize(char *parameter, auth_t auth_level) { // ESP720 + webPrint(parameter); + webPrint("SPIFFS Total:", ESPResponseStream::formatBytes(SPIFFS.totalBytes())); + webPrintln(" Used:", ESPResponseStream::formatBytes(SPIFFS.usedBytes())); + return STATUS_OK; +} + +static err_t formatSpiffs(char *parameter, auth_t auth_level) { // ESP710 + if (strcmp(parameter, "FORMAT") != 0) { + webPrintln("Parameter must be FORMAT"); + return STATUS_INVALID_VALUE; + } + webPrint("Formatting"); + SPIFFS.format(); + webPrintln("...Done"); + return STATUS_OK; +} + +static err_t runFile(char *parameter, auth_t auth_level) { // ESP700 + String path = trim(parameter); + if ((path.length() > 0) && (path[0] != '/')) { + path = "/" + path; + } + if (!SPIFFS.exists(path)) { + webPrintln("Error: No such file!"); + return STATUS_SD_FILE_NOT_FOUND; + } + File currentfile = SPIFFS.open(path, FILE_READ); + if (!currentfile) {//if file open success + return STATUS_SD_FAILED_OPEN_FILE; + } + //until no line in file + err_t err; + err_t accumErr = STATUS_OK; + while (currentfile.available()) { + String currentline = currentfile.readStringUntil('\n'); + if (currentline.length() > 0) { + byte line[256]; + currentline.getBytes(line, 255); + // TODO Settings - feed into command interpreter + // while accumulating error codes + err = execute_line((char *)line, CLIENT_WEBUI, auth_level); + if (err != STATUS_OK) { + accumErr = err; + } + COMMANDS::wait(1); + } + } + currentfile.close(); + return accumErr; +} + +#ifdef ENABLE_NOTIFICATIONS +static err_t showSetNotification(char *parameter, auth_t auth_level) { // ESP610 + if (*parameter == '\0') { + webPrint("", notification_type->getStringValue()); + webPrintln(" ", notification_ts->getStringValue()); + return STATUS_OK; + } + if (!split_params(parameter)) { + return STATUS_INVALID_VALUE; + } + char *ts = get_param("TS", false); + char *t2 = get_param("T2", false); + char *t1 = get_param("T1", false); + char *ty = get_param("type", false); + err_t err = notification_type->setStringValue(ty); + if (!err) { + err = notification_t1->setStringValue(t1); + } + if (!err) { + err = notification_t2->setStringValue(t2); + } + if (!err) { + err = notification_ts->setStringValue(ts); + } + return err; + +} + +static err_t sendMessage(char *parameter, auth_t auth_level) { // ESP600 + if (*parameter == '\0') { + webPrintln("Invalid message!"); + return STATUS_INVALID_VALUE; + } + if (!notificationsservice.sendMSG("GRBL Notification", parameter)) { + webPrintln("Cannot send message!"); + return STATUS_MESSAGE_FAILED; + } + return STATUS_OK; +} +#endif + +#ifdef ENABLE_AUTHENTICATION +static err_t setUserPassword(char *parameter, auth_t auth_level) { // ESP555 + if (*parameter == '\0') { + user_password->setDefault(); + return STATUS_OK; + } + if (user_password->setStringValue(parameter)) { + webPrintln("Invalid Password"); + return STATUS_INVALID_VALUE; + } + return STATUS_OK; +} +#endif + +static err_t setSystemMode(char *parameter, auth_t auth_level) { // ESP444 + parameter = trim(parameter); + if (strcasecmp(parameter, "RESTART") != 0) { + webPrintln("Incorrect command"); + return STATUS_INVALID_VALUE; + } + grbl_send(CLIENT_ALL, "[MSG:Restart ongoing]\r\n"); + COMMANDS::restart_ESP(); + return STATUS_OK; +} + +static err_t showSysStats(char *parameter, auth_t auth_level) { // ESP420 + webPrintln("Chip ID: ", String((uint16_t)(ESP.getEfuseMac() >> 32))); + webPrintln("CPU Frequency: ", String(ESP.getCpuFreqMHz()) + "Mhz"); + webPrintln("CPU Temperature: ", String(temperatureRead(), 1) + "C"); + webPrintln("Free memory: ", ESPResponseStream::formatBytes(ESP.getFreeHeap())); + webPrintln("SDK: ", ESP.getSdkVersion()); + webPrintln("Flash Size: ", ESPResponseStream::formatBytes(ESP.getFlashChipSize())); + + // Round baudRate to nearest 100 because ESP32 can say e.g. 115201 + webPrintln("Baud rate: ", String((Serial.baudRate()/100) * 100)); + webPrintln("Sleep mode: ", WiFi.getSleep() ? "Modem" : "None"); + +#ifdef ENABLE_WIFI + int mode = WiFi.getMode(); + if (mode != WIFI_MODE_NULL) { + //Is OTA available ? + size_t flashsize = 0; + if (esp_ota_get_running_partition()) { + const esp_partition_t* partition = esp_ota_get_next_update_partition(NULL); + if (partition) + flashsize = partition->size; + } + webPrintln("Available Size for update: ", ESPResponseStream::formatBytes(flashsize)); + webPrintln("Available Size for SPIFFS: ", ESPResponseStream::formatBytes(SPIFFS.totalBytes())); + +#if defined (ENABLE_HTTP) + webPrintln("Web port: ", String(web_server.port())); +#endif +#if defined (ENABLE_TELNET) + webPrintln("Data port: ", String(telnet_server.port())); +#endif + webPrintln("Hostname: ", wifi_config.Hostname()); + } + + webPrint("Current WiFi Mode: "); + switch (mode) { + case WIFI_STA: + print_mac("STA", WiFi.macAddress()); + + webPrint("Connected to: "); + if (WiFi.isConnected()) { //in theory no need but ... + webPrintln(WiFi.SSID()); + webPrintln("Signal: ", String(wifi_config.getSignal(WiFi.RSSI())) + "%"); + + uint8_t PhyMode; + esp_wifi_get_protocol(ESP_IF_WIFI_STA, &PhyMode); + const char *modeName; + switch (PhyMode) { + case WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N: + modeName = "11n"; + break; + case WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G: + modeName = "11g"; + break; + case WIFI_PROTOCOL_11B: + modeName = "11b"; + break; + default: + modeName = "???"; + } + webPrintln("Phy Mode: ", modeName); + + webPrintln("Channel: ", String(WiFi.channel())); + + tcpip_adapter_dhcp_status_t dhcp_status; + tcpip_adapter_dhcpc_get_status(TCPIP_ADAPTER_IF_STA, &dhcp_status); + webPrintln("IP Mode: ", dhcp_status == TCPIP_ADAPTER_DHCP_STARTED ? "DHCP" : "Static"); + webPrintln("IP: ", WiFi.localIP()); + webPrintln("Gateway: ", WiFi.gatewayIP()); + webPrintln("Mask: ", WiFi.subnetMask()); + webPrintln("DNS: ", WiFi.dnsIP()); + + } //this is web command so connection => no command + webPrint("Disabled Mode: "); + print_mac("AP", WiFi.softAPmacAddress()); + break; + case WIFI_AP: + print_mac("AP", WiFi.softAPmacAddress()); + + wifi_config_t conf; + esp_wifi_get_config(ESP_IF_WIFI_AP, &conf); + webPrintln("SSID: ", (const char*) conf.ap.ssid); + webPrintln("Visible: ", (conf.ap.ssid_hidden == 0) ? "Yes" : "No"); + + const char *mode; + switch (conf.ap.authmode) { + case WIFI_AUTH_OPEN: mode = "None"; break; + case WIFI_AUTH_WEP: mode = "WEP"; break; + case WIFI_AUTH_WPA_PSK: mode = "WPA"; break; + case WIFI_AUTH_WPA2_PSK: mode = "WPA2"; break; + default: mode = "WPA/WPA2"; + } + + webPrintln("Authentication: ", mode); + webPrintln("Max Connections: ", String(conf.ap.max_connection)); + + tcpip_adapter_dhcp_status_t dhcp_status; + tcpip_adapter_dhcps_get_status(TCPIP_ADAPTER_IF_AP, &dhcp_status); + webPrintln("DHCP Server: ", dhcp_status == TCPIP_ADAPTER_DHCP_STARTED ? "Started" : "Stopped"); + + webPrintln("IP: ", WiFi.softAPIP()); + + tcpip_adapter_ip_info_t ip_AP; + tcpip_adapter_get_ip_info(TCPIP_ADAPTER_IF_AP, &ip_AP); + webPrintln("Gateway: ", IPAddress(ip_AP.gw.addr)); + webPrintln("Mask: ", IPAddress(ip_AP.netmask.addr)); + + wifi_sta_list_t station; + tcpip_adapter_sta_list_t tcpip_sta_list; + esp_wifi_ap_get_sta_list(&station); + tcpip_adapter_get_sta_list(&station, &tcpip_sta_list); + webPrintln("Connected clients: ", String(station.num)); + + for (int i = 0; i < station.num; i++) { + webPrint(wifi_config.mac2str(tcpip_sta_list.sta[i].mac)); + webPrintln(" ", IPAddress(tcpip_sta_list.sta[i].ip.addr)); + } + webPrint("Disabled Mode: "); + print_mac("STA", WiFi.macAddress()); + break; + case WIFI_AP_STA: //we should not be in this state but just in case .... + webPrintln("Mixed"); + + print_mac("STA", WiFi.macAddress()); + print_mac("AP", WiFi.softAPmacAddress()); + break; + default: //we should not be there if no wifi .... + webPrintln("Off"); + break; + } +#endif // ENABLE_WIFI +#ifdef ENABLE_BLUETOOTH + webPrint("Current BT Mode: "); + if (bt_config.Is_BT_on()) { + webPrintln("On"); + + webPrint("BT Name: "); + webPrint(bt_config.BTname()); + webPrint("("); + webPrint(bt_config.device_address()); + webPrintln(")"); + + webPrint("Status: "); + if (SerialBT.hasClient()) { + webPrintln("Connected with ", bt_config._btclient); + } else + webPrintln("Not connected"); + } else + webPrintln("Off"); +#endif +#ifdef ENABLE_NOTIFICATIONS + webPrint("Notifications: "); + webPrint(notificationsservice.started() ? "Enabled" : "Disabled"); + if (notificationsservice.started()) { + webPrint("("); + webPrint(notificationsservice.getTypeString()); + webPrint(")"); + } + webPrintln(""); +#endif + webPrint("FW version: "); + webPrint(GRBL_VERSION); + webPrint(" ("); + webPrint(GRBL_VERSION_BUILD); + webPrint(") (ESP32)"); + webPrintln(""); + return STATUS_OK; +} + +#ifdef ENABLE_WIFI +static err_t listAPs(char *parameter, auth_t auth_level) { // ESP410 + JSONencoder* j = new JSONencoder(espresponse->client() != CLIENT_WEBUI); + j->begin(); + j->begin_array("AP_LIST"); + // An initial async scanNetworks was issued at startup, so there + // is a good chance that scan information is already available. + int n = WiFi.scanComplete(); + switch (n) { + case -2: // Scan not triggered + WiFi.scanNetworks(true); // Begin async scan + break; + case -1: // Scan in progress + break; + default: + for (int i = 0; i < n; ++i) { + j->begin_object(); + j->member("SSID", WiFi.SSID(i)); + j->member("SIGNAL", wifi_config.getSignal(WiFi.RSSI(i))); + j->member("IS_PROTECTED", WiFi.encryptionType(i) != WIFI_AUTH_OPEN); + // j->member("IS_PROTECTED", WiFi.encryptionType(i) == WIFI_AUTH_OPEN ? "0" : "1"); + j->end_object(); + } + WiFi.scanDelete(); + // Restart the scan in async mode so new data will be available + // when we ask again. + n = WiFi.scanComplete(); + if (n == -2) { + WiFi.scanNetworks(true); + } + break; + } + j->end_array(); + webPrint(j->end()); + delete j; + if (espresponse->client() != CLIENT_WEBUI) { + espresponse->println(""); + } + return STATUS_OK; +} +#endif + +static err_t setWebSetting(char *parameter, auth_t auth_level) { // ESP401 + // We do not need the "T=" (type) parameter because the + // Setting objects know their own type + if (!split_params(parameter)) { + return STATUS_INVALID_VALUE; + } + char *sval = get_param("V", true); + const char *spos = get_param("P", false); + if (*spos == '\0') { + webPrintln("Missing parameter"); + return STATUS_INVALID_VALUE; + } + err_t ret = do_command_or_setting(spos, sval, auth_level, espresponse); + return ret; +} + +static err_t listSettings(char *parameter, auth_t auth_level) { // ESP400 + JSONencoder* j = new JSONencoder(espresponse->client() != CLIENT_WEBUI); + j->begin(); + j->begin_array("EEPROM"); + for (Setting *js = Setting::List; js; js = js->next()) { + if (js->getType() == WEBSET) { + js->addWebui(j); + } + } + j->end_array(); + webPrint(j->end()); + delete j; + return STATUS_OK; +} + +#ifdef ENABLE_SD_CARD +static err_t runSDFile(char *parameter, auth_t auth_level) { // ESP220 + parameter = trim(parameter); + if (*parameter == '\0') { + webPrintln("Missing file name!"); + return STATUS_INVALID_VALUE; + } + int8_t state = get_sd_state(true); + if (state != SDCARD_IDLE) { + if (state == SDCARD_NOT_PRESENT) { + webPrintln("No SD Card"); + return STATUS_SD_FAILED_MOUNT; + } else { + webPrintln("SD Card Busy"); + return STATUS_SD_FAILED_BUSY; + } + } + if (sys.state != STATE_IDLE) { + webPrintln("Busy"); + return STATUS_IDLE_ERROR; + } + if (!openFile(SD, parameter)) { + report_status_message(STATUS_SD_FAILED_READ, (espresponse) ? espresponse->client() : CLIENT_ALL); + webPrintln(""); + return STATUS_OK; + } + char fileLine[255]; + if (!readFileLine(fileLine, 255)) { + //No need notification here it is just a macro + closeFile(); + webPrintln(""); + return STATUS_OK; + } + SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL; + report_status_message(gc_execute_line(fileLine, (espresponse) ? espresponse->client() : CLIENT_ALL), (espresponse) ? espresponse->client() : CLIENT_ALL); // execute the first line + report_realtime_status((espresponse) ? espresponse->client() : CLIENT_ALL); + webPrintln(""); + return STATUS_OK; +} + +static err_t deleteSDObject(char *parameter, auth_t auth_level) { // ESP215 + parameter = trim(parameter); + if (*parameter == '\0') { + webPrintln("Missing file name!"); + return STATUS_INVALID_VALUE; + } + int8_t state = get_sd_state(true); + if (state != SDCARD_IDLE) { + webPrintln( (state == SDCARD_NOT_PRESENT) ? "No SD card" : "Busy"); + return STATUS_OK; + } + String path = parameter; + if (parameter[0] != '/') { + path = "/" + path; + } + File file2del = SD.open(path.c_str()); + if (!file2del) { + webPrintln("Cannot stat file!"); + return STATUS_SD_FILE_NOT_FOUND; + } + if (file2del.isDirectory()) { + if (!SD.rmdir((char*)path.c_str())) { + webPrintln("Cannot delete directory! Is directory empty?"); + return STATUS_SD_FAILED_DEL_DIR; + } + webPrintln("Directory deleted."); + } else { + if (!SD.remove((char*)path.c_str())) { + webPrintln("Cannot delete file!"); + return STATUS_SD_FAILED_DEL_FILE; + } + webPrintln("File deleted."); + } + file2del.close(); + return STATUS_OK; +} + +static err_t listSDFiles(char *parameter, auth_t auth_level) { // ESP210 + int8_t state = get_sd_state(true); + if (state != SDCARD_IDLE) { + if (state == SDCARD_NOT_PRESENT) { + webPrintln("No SD Card"); + return STATUS_SD_FAILED_MOUNT; + } else { + webPrintln("SD Card Busy"); + return STATUS_SD_FAILED_BUSY; + } + } + webPrintln(""); + listDir(SD, "/", 10, espresponse->client()); + String ssd = "[SD Free:" + ESPResponseStream::formatBytes(SD.totalBytes() - SD.usedBytes()); + ssd += " Used:" + ESPResponseStream::formatBytes(SD.usedBytes()); + ssd += " Total:" + ESPResponseStream::formatBytes(SD.totalBytes()); + ssd += "]"; + webPrintln(ssd); + return STATUS_OK; +} +#endif + +static err_t listLocalFiles(char *parameter, auth_t auth_level) { // No ESP command + webPrintln(""); + listDir(SPIFFS, "/", 10, espresponse->client()); + String ssd = "[Local FS Free:" + ESPResponseStream::formatBytes(SPIFFS.totalBytes() - SPIFFS.usedBytes()); + ssd += " Used:" + ESPResponseStream::formatBytes(SPIFFS.usedBytes()); + ssd += " Total:" + ESPResponseStream::formatBytes(SPIFFS.totalBytes()); + ssd += "]"; + webPrintln(ssd); + return STATUS_OK; +} + +static void listDirJSON(fs::FS& fs, const char* dirname, uint8_t levels, JSONencoder* j) { + File root = fs.open(dirname); + File file = root.openNextFile(); + while (file) { + const char* tailName = strchr(file.name(), '/'); + tailName = tailName ? tailName + 1 : file.name(); + if (file.isDirectory() && levels) { + j->begin_array(tailName); + listDirJSON(fs, file.name(), levels - 1, j); + j->end_array(); + } else { + j->begin_object(); + j->member("name", tailName); + j->member("size", file.size()); + j->end_object(); + } + file = root.openNextFile(); + } +} + +static err_t listLocalFilesJSON(char *parameter, auth_t auth_level) { // No ESP command + JSONencoder* j = new JSONencoder(espresponse->client() != CLIENT_WEBUI); + j->begin(); + j->begin_array("files"); + listDirJSON(SPIFFS, "/", 4, j); + j->end_array(); + j->member("total", SPIFFS.totalBytes()); + j->member("used", SPIFFS.usedBytes()); + j->member("occupation", String(100 * SPIFFS.usedBytes() / SPIFFS.totalBytes())); + webPrint(j->end()); + if (espresponse->client() != CLIENT_WEBUI) { + webPrintln(""); + } + return STATUS_OK; +} + +static err_t showSDStatus(char *parameter, auth_t auth_level) { // ESP200 + const char* resp = "No SD card"; +#ifdef ENABLE_SD_CARD + switch (get_sd_state(true)) { + case SDCARD_IDLE: + resp = "SD card detected"; + break; + case SDCARD_NOT_PRESENT: + resp = "No SD card"; + break; + default: + resp = "Busy"; + } +#endif + webPrintln(resp); + return STATUS_OK; +} + +static err_t setRadioState(char *parameter, auth_t auth_level) { // ESP115 + parameter = trim(parameter); + if (*parameter == '\0') { + // Display the radio state + bool on = false; +#if defined (ENABLE_WIFI) + if (WiFi.getMode() != WIFI_MODE_NULL)on = true; +#endif +#if defined (ENABLE_BLUETOOTH) + if (bt_config.Is_BT_on())on = true; +#endif + webPrintln(on ? "ON" : "OFF"); + return STATUS_OK; + } + int8_t on = -1; + if (strcasecmp(parameter, "ON") == 0) { + on = 1; + } else if (strcasecmp(parameter, "OFF") == 0) { + on = 0; + } + if (on == -1) { + webPrintln("only ON or OFF mode supported!"); + return STATUS_INVALID_VALUE; + } + //Stop everything +#if defined (ENABLE_WIFI) + if (WiFi.getMode() != WIFI_MODE_NULL) { + wifi_config.StopWiFi(); + } +#endif +#if defined (ENABLE_BLUETOOTH) + if (bt_config.Is_BT_on()) { + bt_config.end(); + } +#endif + //if On start proper service + if (!on) { + webPrintln("[MSG: Radio is Off]"); + return STATUS_OK; + } + //On +#ifdef WIFI_OR_BLUETOOTH + switch (wifi_radio_mode->get()) { + case ESP_WIFI_AP: + case ESP_WIFI_STA: + #if !defined (ENABLE_WIFI) + webPrintln("WiFi is not enabled!"); + return STATUS_WIFI_FAIL_BEGIN; + + #else + wifi_config.begin(); + return STATUS_OK; + #endif + case ESP_BT: + #if !defined (ENABLE_BLUETOOTH) + webPrintln("Bluetooth is not enabled!"); + return STATUS_BT_FAIL_BEGIN; + #else + bt_config.begin(); + return STATUS_OK; + #endif + default: + webPrintln("[MSG: Radio is Off]"); + return STATUS_OK; + } +#endif + return STATUS_OK; +} + +#ifdef ENABLE_WIFI +static err_t showIP(char *parameter, auth_t auth_level) { // ESP111 + webPrintln(parameter, WiFi.getMode() == WIFI_STA ? WiFi.localIP() : WiFi.softAPIP()); + return STATUS_OK; +} + +static err_t showSetStaParams(char *parameter, auth_t auth_level) { // ESP103 + if (*parameter == '\0') { + webPrint("IP:", wifi_sta_ip->getStringValue()); + webPrint(" GW:", wifi_sta_gateway->getStringValue()); + webPrintln(" MSK:", wifi_sta_netmask->getStringValue()); + return STATUS_OK; + } + if (!split_params(parameter)) { + return STATUS_INVALID_VALUE; + } + char *gateway = get_param("GW", false); + char *netmask = get_param("MSK", false); + char *ip = get_param("IP", false); + + err_t err = wifi_sta_ip->setStringValue(ip); + if (!err) { + err = wifi_sta_netmask->setStringValue(netmask); + } + if (!err) { + err = wifi_sta_gateway->setStringValue(gateway); + } + return err; +} +#endif + +static err_t showWebHelp(char *parameter, auth_t auth_level) { // ESP0 + webPrintln("Persistent web settings - $name to show, $name=value to set"); + webPrintln("ESPname FullName Description"); + webPrintln("------- -------- -----------"); + for (Setting *s = Setting::List; s; s = s->next()) { + if (s->getType() == WEBSET) { + if (s->getGrblName()) { + webPrint(" ", s->getGrblName()); + } + webPrintSetColumn(8); + webPrint(s->getName()); + webPrintSetColumn(25); + webPrintln(s->getDescription()); + } + } + webPrintln(""); + webPrintln("Other web commands: $name to show, $name=value to set"); + webPrintln("ESPname FullName Values"); + webPrintln("------- -------- ------"); + for (Command *cp = Command::List; cp; cp = cp->next()) { + if (cp->getType() == WEBCMD) { + if (cp->getGrblName()) { + webPrint(" ", cp->getGrblName()); + } + webPrintSetColumn(8); + webPrint(cp->getName()); + if (cp->getDescription()) { + webPrintSetColumn(25); + webPrintln(cp->getDescription()); + } else { + webPrintln(""); + } + } + } + return STATUS_OK; +} + +// WEB_COMMON should always be defined. It is a trick to make the definitions +// line up while allowing VSCode code folding to work correction. +#define WEB_COMMON + +void make_web_settings() +{ + // If authentication enabled, display_settings skips or displays + // RU - need user or admin password to read + // WU - need user or admin password to set + // WA - need admin password to set + #ifdef WEB_COMMON + new WebCommand(NULL, WEBCMD, WG, "ESP800", "Firmware/Info", showFwInfo); + new WebCommand(NULL, WEBCMD, WU, "ESP720", "LocalFS/Size", SPIFFSSize); + new WebCommand("FORMAT", WEBCMD, WA, "ESP710", "LocalFS/Format", formatSpiffs); + new WebCommand("path", WEBCMD, WU, "ESP700", "LocalFS/Run", runFile); + new WebCommand("path", WEBCMD, WU, NULL, "LocalFS/List", listLocalFiles); + new WebCommand("path", WEBCMD, WU, NULL, "LocalFS/ListJSON",listLocalFilesJSON); + #endif + #ifdef ENABLE_NOTIFICATIONS + new WebCommand("TYPE=NONE|PUSHOVER|EMAIL|LINE T1=token1 T2=token2 TS=settings", + WEBCMD, WA, "ESP610", "Notification/Setup",showSetNotification); + new WebCommand("message", WEBCMD, WU, "ESP600", "Notification/Send", sendMessage); + #endif + #ifdef ENABLE_AUTHENTICATION + new WebCommand("password",WEBCMD, WA, "ESP555", "WebUI/SetUserPassword", setUserPassword); + #endif + #ifdef WEB_COMMON + new WebCommand("RESTART", WEBCMD, WA, "ESP444", "System/Control",setSystemMode); + new WebCommand(NULL, WEBCMD, WU, "ESP420", "System/Stats", showSysStats); + #endif + #ifdef ENABLE_WIFI + new WebCommand(NULL, WEBCMD, WU, "ESP410", "WiFi/ListAPs", listAPs); + #endif + #ifdef WEB_COMMON + new WebCommand("P=position T=type V=value", + WEBCMD, WA, "ESP401", "WebUI/Set", setWebSetting); + new WebCommand(NULL, WEBCMD, WU, "ESP400", "WebUI/List", listSettings); + #endif + #ifdef ENABLE_SD_CARD + new WebCommand("path", WEBCMD, WU, "ESP220", "SD/Run", runSDFile); + new WebCommand("file_or_directory_path", + WEBCMD, WU, "ESP215", "SD/Delete", deleteSDObject); + new WebCommand(NULL, WEBCMD, WU, "ESP210", "SD/List", listSDFiles); + #endif + #ifdef WEB_COMMON + new WebCommand(NULL, WEBCMD, WU, "ESP200", "SD/Status", showSDStatus); + new WebCommand("STA|AP|BT|OFF", + WEBCMD, WA, "ESP115", "Radio/State", setRadioState); + #endif + #ifdef ENABLE_WIFI + new WebCommand(NULL, WEBCMD, WG,"ESP111", "System/IP", showIP); + new WebCommand("IP=ipaddress MSK=netmask GW=gateway", + WEBCMD, WA, "ESP103", "Sta/Setup", showSetStaParams); + #endif + #ifdef WEB_COMMON + new WebCommand(NULL, WEBCMD, WG, "ESP0", "WebUI/Help", showWebHelp); + new WebCommand(NULL, WEBCMD, WG, "ESP", "WebUI/Help", showWebHelp); + #endif + // WebUI Settings + // Standard WEBUI authentication is user+ to get, admin to set unless otherwise specified + #ifdef ENABLE_NOTIFICATIONS + notification_ts = new StringSetting("Notification Settings", WEBSET, WA, NULL, "Notification/TS", DEFAULT_TOKEN, 0, MAX_NOTIFICATION_SETTING_LENGTH, NULL); + notification_t2 = new StringSetting("Notification Token 2", WEBSET, WA, NULL, "Notification/T2", DEFAULT_TOKEN, MIN_NOTIFICATION_TOKEN_LENGTH, MAX_NOTIFICATION_TOKEN_LENGTH, NULL); + notification_t1 = new StringSetting("Notification Token 1", WEBSET, WA, NULL, "Notification/T1", DEFAULT_TOKEN , MIN_NOTIFICATION_TOKEN_LENGTH, MAX_NOTIFICATION_TOKEN_LENGTH, NULL); + notification_type = new EnumSetting("Notification type", WEBSET, WA, NULL, "Notification/Type", DEFAULT_NOTIFICATION_TYPE, ¬ificationOptions); + #endif + #ifdef ENABLE_AUTHENTICATION + user_password = new StringSetting("User password", WEBSET, WA, NULL, "WebUI/UserPassword", DEFAULT_USER_PWD, MIN_LOCAL_PASSWORD_LENGTH, MAX_LOCAL_PASSWORD_LENGTH, &COMMANDS::isLocalPasswordValid); + admin_password = new StringSetting("Admin password", WEBSET, WA, NULL, "WebUI/AdminPassword", DEFAULT_ADMIN_PWD, MIN_LOCAL_PASSWORD_LENGTH, MAX_LOCAL_PASSWORD_LENGTH, &COMMANDS::isLocalPasswordValid); + #endif + #ifdef ENABLE_BLUETOOTH + bt_name = new StringSetting("Bluetooth name", WEBSET, WA, "ESP140", "Bluetooth/Name", DEFAULT_BT_NAME, MIN_BTNAME_LENGTH, MAX_BTNAME_LENGTH, (bool (*)(char*))BTConfig::isBTnameValid); + #endif + + #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); + #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); + 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); + wifi_hostname = new StringSetting("Hostname", WEBSET, WA, "ESP112", "System/Hostname", DEFAULT_HOSTNAME, MIN_HOSTNAME_LENGTH, MAX_HOSTNAME_LENGTH, (bool (*)(char*))WiFiConfig::isHostnameValid); + wifi_ap_channel = new IntSetting("AP Channel", WEBSET, WA, "ESP108", "AP/Channel", DEFAULT_AP_CHANNEL, MIN_CHANNEL, MAX_CHANNEL, NULL); + wifi_ap_ip = new IPaddrSetting("AP Static IP", WEBSET, WA, "ESP107", "AP/IP", DEFAULT_AP_IP, NULL); + // no get, admin to set + wifi_ap_password = new StringSetting("AP Password", WEBSET, WA, "ESP106", "AP/Password", DEFAULT_AP_PWD, MIN_PASSWORD_LENGTH, MAX_PASSWORD_LENGTH, (bool (*)(char*))WiFiConfig::isPasswordValid); + wifi_ap_ssid = new StringSetting("AP SSID", WEBSET, WA, "ESP105", "AP/SSID", DEFAULT_AP_SSID, MIN_SSID_LENGTH, MAX_SSID_LENGTH, (bool (*)(char*))WiFiConfig::isSSIDValid); + 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); + // no get, admin to set + wifi_sta_password = new StringSetting("Station Password", WEBSET, WA, "ESP101", "Sta/Password", DEFAULT_STA_PWD, MIN_PASSWORD_LENGTH, MAX_PASSWORD_LENGTH, (bool (*)(char*))WiFiConfig::isPasswordValid); + wifi_sta_ssid = new StringSetting("Station SSID", WEBSET, WA, "ESP100", "Sta/SSID", DEFAULT_STA_SSID, MIN_SSID_LENGTH, MAX_SSID_LENGTH, (bool (*)(char*))WiFiConfig::isSSIDValid); + #endif +} diff --git a/Grbl_Esp32/WebSettings.h b/Grbl_Esp32/WebSettings.h new file mode 100644 index 00000000..8fea8fc7 --- /dev/null +++ b/Grbl_Esp32/WebSettings.h @@ -0,0 +1,64 @@ +/* + WebSettings.h - Definitions for WebUI-related settings. + + Copyright (c) 2020 Mitch Bradley + Copyright (c) 2014 Luc Lebosse. All rights reserved. + + 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 . +*/ + +#pragma once + +extern StringSetting* wifi_sta_ssid; +extern StringSetting* wifi_sta_password; + +#ifdef ENABLE_WIFI +extern EnumSetting* wifi_sta_mode; +extern IPaddrSetting* wifi_sta_ip; +extern IPaddrSetting* wifi_sta_gateway; +extern IPaddrSetting* wifi_sta_netmask; + +extern StringSetting* wifi_ap_ssid; +extern StringSetting* wifi_ap_password; + +extern IPaddrSetting* wifi_ap_ip; + +extern IntSetting* wifi_ap_channel; + +extern StringSetting* wifi_hostname; +extern EnumSetting* http_enable; +extern IntSetting* http_port; +extern EnumSetting* telnet_enable; +extern IntSetting* telnet_port; +#endif + +#ifdef WIFI_OR_BLUETOOTH +extern EnumSetting* wifi_radio_mode; +#endif + +#ifdef ENABLE_BLUETOOTH +extern StringSetting* bt_name; +#endif + +#ifdef ENABLE_AUTHENTICATION +extern StringSetting* user_password; +extern StringSetting* admin_password; +#endif + +#ifdef ENABLE_NOTIFICATIONS +extern EnumSetting* notification_type; +extern StringSetting* notification_t1; +extern StringSetting* notification_t2; +extern StringSetting* notification_ts; +#endif diff --git a/Grbl_Esp32/authentication.cpp b/Grbl_Esp32/authentication.cpp new file mode 100644 index 00000000..59f75c6f --- /dev/null +++ b/Grbl_Esp32/authentication.cpp @@ -0,0 +1,41 @@ +#include "grbl.h" + +#ifdef ENABLE_AUTHENTICATION +// TODO Settings - need ADMIN_ONLY and if it is called without a parameter it sets the default +StringSetting* user_password; +StringSetting* admin_password; + +void remove_password(char *str, auth_t& auth_level) { + String paramStr = String((const char*)str); + int pos = paramStr.indexOf("pwd="); + if (pos == -1) { + return; + } + + // Truncate the str string at the pwd= . + // If the pwd= is preceded by a space, take off that space too. + int endpos = pos; + if (endpos && str[endpos-1] == ' ') { + --endpos; + } + str[endpos] = '\0'; + + // Upgrade the authentication level if a password + // for a higher level is present. + const char* password = str + pos + strlen("pwd="); + if (auth_level < LEVEL_USER) { + if (!strcmp(password, user_password->get())) { + auth_level = LEVEL_USER; + } + } + if (auth_level < LEVEL_ADMIN) { + if (!strcmp(password, admin_password->get())) { + auth_level = LEVEL_ADMIN; + } + } +} +#else +void remove_password(char *str, auth_t& auth_level) { + auth_level = LEVEL_ADMIN; +} +#endif diff --git a/Grbl_Esp32/authentication.h b/Grbl_Esp32/authentication.h new file mode 100644 index 00000000..09ca20eb --- /dev/null +++ b/Grbl_Esp32/authentication.h @@ -0,0 +1,13 @@ +#pragma once + +//Authentication level +typedef enum { + LEVEL_GUEST = 0, + LEVEL_USER = 1, + LEVEL_ADMIN = 2 +} auth_t; + +#define MIN_LOCAL_PASSWORD_LENGTH 1 +#define MAX_LOCAL_PASSWORD_LENGTH 16 + +void remove_password(char *str, auth_t& auth_level); diff --git a/Grbl_Esp32/commands.cpp b/Grbl_Esp32/commands.cpp index efd5968f..62791b71 100644 --- a/Grbl_Esp32/commands.cpp +++ b/Grbl_Esp32/commands.cpp @@ -17,34 +17,7 @@ License along with this library; if not, write to the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */ -#include "config.h" -#include "commands.h" -#include "espresponse.h" -#include -#include "report.h" -#ifdef ENABLE_SD_CARD - #include "grbl_sd.h" -#endif -#ifdef ENABLE_BLUETOOTH - #include "BTconfig.h" -#endif -#ifdef ENABLE_WIFI - #include "wificonfig.h" - #if defined (ENABLE_HTTP) - #include "web_server.h" - #endif - #ifdef ENABLE_TELNET - #include "telnet_server.h" - #endif -#endif -#ifdef ENABLE_NOTIFICATIONS - #include "notifications_service.h" -#endif -#include -#include -#include -#include -#include +#include "grbl.h" #ifdef __cplusplus extern "C" { @@ -67,1987 +40,7 @@ void COMMANDS::wait(uint32_t milliseconds) { esp_task_wdt_reset(); } -bool COMMANDS::execute_internal_command(int cmd, String cmd_params, level_authenticate_type auth_level, ESPResponseStream* espresponse) { - bool response = true; - level_authenticate_type auth_type = auth_level; - if (!espresponse) return false; -#ifdef ENABLE_AUTHENTICATION - if (isadmin(cmd_params)) - auth_type = LEVEL_ADMIN; - if (isuser(cmd_params) && (auth_type != LEVEL_ADMIN)) - auth_type = LEVEL_USER; -#endif - //manage parameters - String parameter; - switch (cmd) { - //help - //[ESP] or [ESP0] - case 0: { - if (espresponse) { - espresponse->println("[List of ESP3D commands]"); - espresponse->println("[ESP] - display this help"); - espresponse->println("[ESP100](SSID) - display/set STA SSID"); - espresponse->println("[ESP101](Password) - set STA password"); - espresponse->println("[ESP102](Mode) - display/set STA IP mode (DHCP/STATIC)"); - espresponse->println("[ESP103](IP=xxxx MSK=xxxx GW=xxxx) - display/set STA IP/Mask/GW"); - espresponse->println("[ESP105](SSID) - display/set AP SSID"); - espresponse->println("[ESP106](Password) - set AP password"); - espresponse->println("[ESP107](IP) - display/set AP IP"); - espresponse->println("[ESP108](Chanel) - display/set AP chanel"); - espresponse->println("[ESP110](State) - display/set radio state which can be STA, AP, BT, OFF"); - espresponse->println("[ESP111]display current IP"); - espresponse->println("[ESP112](Hostname) - display/set Hostname"); - espresponse->println("[ESP115](State) - display/set immediate radio state which can be ON, OFF"); - espresponse->println("[ESP120](State) - display/set HTTP state which can be ON, OFF"); - espresponse->println("[ESP121](Port) - display/set HTTP port "); - espresponse->println("[ESP130](State) - display/set Telnet state which can be ON, OFF"); - espresponse->println("[ESP131](Port) - display/set Telnet port "); - espresponse->println("[ESP140](Bluetooth name) - display/set Bluetooth name"); - espresponse->println("[ESP200] - display SD Card Status"); - espresponse->println("[ESP210] - display SD Card content"); - espresponse->println("[ESP215](file/dir name) - delete SD Card file / directory"); - espresponse->println("[ESP220](file name) - run file from SD"); - espresponse->println("[ESP400] - display ESP3D settings in JSON"); - espresponse->println("[ESP401]P=(position) T=(type) V=(value) - Set specific setting"); - espresponse->println("[ESP410] - display available AP list (limited to 30) in JSON"); - espresponse->println("[ESP420] - display ESP3D current status"); - espresponse->println("[ESP444]RESTART - Restart ESP"); -#ifdef ENABLE_AUTHENTICATION - espresponse->println("[ESP555](Password) - set user password"); -#endif //ENABLE_AUTHENTICATION -#ifdef ENABLE_NOTIFICATIONS - espresponse->println("[ESP600](message) - send message"); - espresponse->println("[ESP610]type=(NONE/PUSHOVER/EMAIL/LINE) T1=(token1) T2=(token2) TS=(Settings) - display/set Notification settings"); -#endif //ENABLE_NOTIFICATIONS - espresponse->println("[ESP700](file name) - run macro file from ESP Filesystem"); - espresponse->println("[ESP710]FORMAT - Format ESP Filesystem"); - espresponse->println("[ESP720]display total size and used size of ESP Filesystem"); - espresponse->println("[ESP800] - display FW Informations"); - } - } - break; -#ifdef ENABLE_WIFI - //STA SSID - //[ESP100][pwd=] - case 100: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - String defV = DEFAULT_STA_SSID; - espresponse->println(prefs.getString(STA_SSID_ENTRY, defV).c_str()); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - if (!WiFiConfig::isSSIDValid(parameter.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect SSID!"); - response = false; - } else { - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(STA_SSID_ENTRY, parameter) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - } - break; - //STA Password - //[ESP101][pwd=] - case 101: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if (!WiFiConfig::isPasswordValid(parameter.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect password!"); - response = false; - return false; - } else { - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(STA_PWD_ENTRY, parameter) != parameter.length()) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - break; - //Change STA IP mode (DHCP/STATIC) - //[ESP102]pwd= - case 102: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - int8_t resp = prefs.getChar(STA_IP_MODE_ENTRY, DHCP_MODE); - if (resp == DHCP_MODE) espresponse->println("DHCP"); - else if (resp == STATIC_MODE) espresponse->println("STATIC"); - else espresponse->println("???"); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter.toUpperCase(); - if (!((parameter == "STATIC") || (parameter == "DHCP"))) { - if (espresponse)espresponse->println("Error: only STATIC or DHCP mode supported!"); - response = false; - return false; - } else { - Preferences prefs; - prefs.begin(NAMESPACE, false); - int8_t bbuf = (parameter == "DHCP") ? DHCP_MODE : STATIC_MODE; - if (prefs.putChar(STA_IP_MODE_ENTRY, bbuf) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - } - break; - //Change STA IP/Mask/GW - //[ESP103]IP= MSK= GW= pwd= - case 103: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - //IP - String defV = DEFAULT_STA_IP; - int32_t IP = prefs.getInt(STA_IP_ENTRY, wifi_config.IP_int_from_string(defV)); - //GW - defV = DEFAULT_STA_GW; - int32_t GW = prefs.getInt(STA_GW_ENTRY, wifi_config.IP_int_from_string(defV)); - //MK - defV = DEFAULT_STA_MK; - int32_t MK = prefs.getInt(STA_MK_ENTRY, wifi_config.IP_int_from_string(defV)); - defV = "IP:" + wifi_config.IP_string_from_int(IP) + ", GW:" + wifi_config.IP_string_from_int(GW) + ", MSK:" + wifi_config.IP_string_from_int(MK); - espresponse->println(defV.c_str()); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - String IP = get_param(cmd_params, "IP=", false); - String GW = get_param(cmd_params, "GW=", false); - String MSK = get_param(cmd_params, "MSK=", false); - Serial.println(IP); - Serial.println(GW); - if (!WiFiConfig::isValidIP(IP.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect IP!"); - response = false; - return false; - } - if (!WiFiConfig::isValidIP(GW.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect Gateway!"); - response = false; - return false; - } - if (!WiFiConfig::isValidIP(MSK.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect Mask!"); - response = false; - return false; - } - Preferences prefs; - prefs.begin(NAMESPACE, false); - if ((prefs.putInt(STA_IP_ENTRY, wifi_config.IP_int_from_string(IP)) == 0) || - (prefs.putInt(STA_GW_ENTRY, wifi_config.IP_int_from_string(GW)) == 0) || - (prefs.putInt(STA_MK_ENTRY, wifi_config.IP_int_from_string(MSK)) == 0)) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - break; - //Change AP SSID - //[ESP105]pwd= - case 105: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - String defV = DEFAULT_AP_SSID; - espresponse->println(prefs.getString(AP_SSID_ENTRY, defV).c_str()); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - if (!WiFiConfig::isSSIDValid(parameter.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect SSID!"); - response = false; - } - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(AP_SSID_ENTRY, parameter) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - break; - //Change AP Password - //[ESP106]pwd= - case 106: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if (!WiFiConfig::isPasswordValid(parameter.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect password!"); - response = false; - return false; - } - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(AP_PWD_ENTRY, parameter) != parameter.length()) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - break; - //Change AP IP - //[ESP107]pwd= - case 107: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - //IP - String defV = DEFAULT_AP_IP; - int32_t IP = prefs.getInt(AP_IP_ENTRY, wifi_config.IP_int_from_string(defV)); - espresponse->println(wifi_config.IP_string_from_int(IP).c_str()); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - if (!WiFiConfig::isValidIP(parameter.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect IP!"); - response = false; - return false; - } - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putInt(AP_IP_ENTRY, wifi_config.IP_int_from_string(parameter)) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - break; - //Change AP channel - //[ESP108]pwd= - case 108: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - int8_t channel = prefs.getChar(AP_CHANNEL_ENTRY, DEFAULT_AP_CHANNEL); - espresponse->println(String(channel).c_str()); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - int8_t bbuf = parameter.toInt(); - if ((bbuf > MAX_CHANNEL) || (bbuf < MIN_CHANNEL)) { - if (espresponse)espresponse->println("Error: Incorrect channel!"); - response = false; - return false; - } - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putChar(AP_CHANNEL_ENTRY, bbuf) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - break; -#endif -#if defined( ENABLE_WIFI) || defined( ENABLE_BLUETOOTH) - //Set radio state at boot which can be BT, STA, AP, OFF - //[ESP110]pwd= - case 110: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - int8_t wifiMode = prefs.getChar(ESP_RADIO_MODE, DEFAULT_RADIO_MODE); - if (wifiMode == ESP_RADIO_OFF) espresponse->println("OFF"); - else if (wifiMode == ESP_BT) espresponse->println("BT"); - else if (wifiMode == ESP_WIFI_AP) espresponse->println("AP"); - else if (wifiMode == ESP_WIFI_STA) espresponse->println("STA"); - else espresponse->println("??"); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter.toUpperCase(); - if (!( -#if defined( ENABLE_BLUETOOTH) - (parameter == "BT") || -#endif -#if defined( ENABLE_WIFI) - (parameter == "STA") || (parameter == "AP") || -#endif - (parameter == "OFF"))) { - if (espresponse)espresponse->println("Error: only " -#ifdef ENABLE_BLUETOOTH - "BT or " -#endif -#ifdef ENABLE_WIFI - "STA or AP or " -#endif - "OFF mode supported!"); - response = false; - return false; - } else { - Preferences prefs; - prefs.begin(NAMESPACE, false); - int8_t bbuf = ESP_RADIO_OFF; -#ifdef ENABLE_WIFI - if (parameter == "STA")bbuf = ESP_WIFI_STA; - if (parameter == "AP")bbuf = ESP_WIFI_AP; -#endif -#ifdef ENABLE_BLUETOOTH - if (parameter == "BT")bbuf = ESP_BT; -#endif - if (prefs.putChar(ESP_RADIO_MODE, bbuf) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - } - break; -#endif -#ifdef ENABLE_WIFI - //Get current IP - //[ESP111]
- case 111: { - if (!espresponse) return false; - String currentIP = cmd_params; - if (WiFi.getMode() == WIFI_STA) - currentIP += WiFi.localIP().toString(); - else - currentIP += WiFi.softAPIP().toString(); - espresponse->println(currentIP.c_str()); - } - break; - //Get/Set hostname - //[ESP112] pwd= - case 112: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //Get hostname - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - String defV = DEFAULT_HOSTNAME; - espresponse->println(prefs.getString(HOSTNAME_ENTRY, defV).c_str()); - prefs.end(); - } else { //set host name -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - if (!wifi_config.isHostnameValid(parameter.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect hostname!"); - response = false; - } else { - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(HOSTNAME_ENTRY, parameter) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - } - break; -#endif -#if defined (ENABLE_WIFI) || defined (ENABLE_BLUETOOTH) - //Set immediate radio state which can be ON, OFF - //[ESP115]pwd= - case 115: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - //get - if (parameter.length() == 0) { - bool on = false; -#if defined (ENABLE_WIFI) - if (WiFi.getMode() != WIFI_MODE_NULL)on = true; -#endif -#if defined (ENABLE_BLUETOOTH) - if (bt_config.Is_BT_on())on = true; -#endif - espresponse->println((on) ? "ON" : "OFF"); - } else { - parameter.toUpperCase(); - if (!((parameter == "ON") || (parameter == "OFF"))) { - if (espresponse)espresponse->println("Error: only ON or OFF mode supported!"); - return false; - } else { - //Stop everything -#if defined (ENABLE_WIFI) - if (WiFi.getMode() != WIFI_MODE_NULL)wifi_config.StopWiFi(); -#endif -#if defined (ENABLE_BLUETOOTH) - if (bt_config.Is_BT_on())bt_config.end(); -#endif - //if On start proper service - if (parameter == "ON") { //On - Preferences prefs; - prefs.begin(NAMESPACE, true); - int8_t wifiMode = prefs.getChar(ESP_RADIO_MODE, DEFAULT_RADIO_MODE); - prefs.end(); - if ((wifiMode == ESP_WIFI_AP) || (wifiMode == ESP_WIFI_STA)) { -#if defined (ENABLE_WIFI) - wifi_config.begin(); -#else - if (espresponse)espresponse->println("Error: WiFi is not enabled!"); - return false; -#endif - } else if (wifiMode == ESP_BT) { -#if defined (ENABLE_BLUETOOTH) - bt_config.begin(); -#else - if (espresponse)espresponse->println("Error: Bluetooth is not enabled!"); - return false; -#endif - } else { - if (espresponse)espresponse->println("[MSG: Radio is Off]"); - return false; - } - } else if (espresponse)espresponse->println("[MSG: Radio is Off]"); - } - } - } - break; -#endif -#ifdef ENABLE_WIFI - //Set HTTP state which can be ON, OFF - //[ESP120]pwd= - case 120: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - int8_t Mode = prefs.getChar(HTTP_ENABLE_ENTRY, DEFAULT_HTTP_STATE); - espresponse->println((Mode == 0) ? "OFF" : "ON"); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter.toUpperCase(); - if (!((parameter == "ON") || (parameter == "OFF"))) { - if (espresponse)espresponse->println("Error: only ON or OFF mode supported!"); - response = false; - return false; - } else { - Preferences prefs; - prefs.begin(NAMESPACE, false); - int8_t bbuf = (parameter == "ON") ? 1 : 0; - if (prefs.putChar(HTTP_ENABLE_ENTRY, bbuf) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - } - break; - //Set HTTP port - //[ESP121]pwd= - case 121: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - int port = prefs.getUShort(HTTP_PORT_ENTRY, DEFAULT_WEBSERVER_PORT); - espresponse->println(String(port).c_str()); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - int ibuf = parameter.toInt(); - if ((ibuf > MAX_HTTP_PORT) || (ibuf < MIN_HTTP_PORT)) { - if (espresponse)espresponse->println("Error: Incorrect port!"); - response = false; - return false; - } - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putUShort(HTTP_PORT_ENTRY, ibuf) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - break; - //Set Telnet state which can be ON, OFF - //[ESP130]pwd= - case 130: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - int8_t Mode = prefs.getChar(TELNET_ENABLE_ENTRY, DEFAULT_TELNET_STATE); - espresponse->println((Mode == 0) ? "OFF" : "ON"); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter.toUpperCase(); - if (!((parameter == "ON") || (parameter == "OFF"))) { - if (espresponse)espresponse->println("Error: only ON or OFF mode supported!"); - response = false; - return false; - } else { - Preferences prefs; - prefs.begin(NAMESPACE, false); - int8_t bbuf = (parameter == "ON") ? 1 : 0; - if (prefs.putChar(TELNET_ENABLE_ENTRY, bbuf) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - } - break; - //Set Telnet port - //[ESP131]pwd= - case 131: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - int port = prefs.getUShort(TELNET_PORT_ENTRY, DEFAULT_TELNETSERVER_PORT); - espresponse->println(String(port).c_str()); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - int ibuf = parameter.toInt(); - if ((ibuf > MAX_TELNET_PORT) || (ibuf < MIN_TELNET_PORT)) { - if (espresponse)espresponse->println("Error: Incorrect port!"); - response = false; - return false; - } - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putUShort(TELNET_PORT_ENTRY, ibuf) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - break; -#endif -#ifdef ENABLE_BLUETOOTH - //Get/Set btname - //[ESP140]< Bluetooth name> pwd= - case 140: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; -#ifdef ENABLE_AUTHENTICATION - if ((auth_type != LEVEL_ADMIN) && (parameter.length() > 0)) { - espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - //Get btname - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - String defV = DEFAULT_BT_NAME; - espresponse->println(prefs.getString(BT_NAME_ENTRY, defV).c_str()); - prefs.end(); - } else { //set BT name - if (!bt_config.isBTnameValid(parameter.c_str())) { - if (espresponse)espresponse->println("Error: Incorrect name!"); - response = false; - } else { - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(BT_NAME_ENTRY, parameter) == 0) { - response = false; - if (espresponse)espresponse->println("Error: Set failed!"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } - } - } - break; -#endif - //Get SD Card Status - //[ESP200] - case 200: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - if (!espresponse) return false; - String resp = "No SD card"; -#ifdef ENABLE_SD_CARD - int8_t state = get_sd_state(true); - if (state == SDCARD_IDLE)resp = "SD card detected"; - else if (state == SDCARD_NOT_PRESENT)resp = "No SD card"; - else resp = "Busy"; -#endif - espresponse->println(resp.c_str()); - } - break; -#ifdef ENABLE_SD_CARD - //Get SD Card Content - //[ESP210] - case 210: { - if (!espresponse) return false; -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - int8_t state = get_sd_state(true); - if (state == SDCARD_IDLE) { - listDir(SD, "/", 10, espresponse->client()); - String ssd = "[SD Free:" + ESPResponseStream::formatBytes(SD.totalBytes() - SD.usedBytes()); - ssd += " Used:" + ESPResponseStream::formatBytes(SD.usedBytes()); - ssd += " Total:" + ESPResponseStream::formatBytes(SD.totalBytes()); - ssd += "]"; - espresponse->println(""); - espresponse->println(ssd.c_str()); - } else espresponse->println((state == SDCARD_NOT_PRESENT) ? "No SD card" : "Busy"); - } - break; - //Delete SD Card file / directory - //[ESP215]pwd= - case 215: { - if (!espresponse) return false; -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if (parameter.length() != 0) { - int8_t state = get_sd_state(true); - parameter.trim(); - if (parameter[0] != '/') - parameter = "/" + parameter; - if (state == SDCARD_IDLE) { - File file2del = SD.open(parameter.c_str()); - if (file2del) { - if (file2del.isDirectory()) { - if (!SD.rmdir((char*)parameter.c_str())) - espresponse->println("Error: Cannot delete directory! Is directory empty?"); - else - espresponse->println("Directory deleted."); - } else { - if (!SD.remove((char*)parameter.c_str())) - espresponse->println("Error: Cannot delete file!"); - else - espresponse->println("File deleted."); - } - } else - espresponse->println("Error: Cannot stat file!"); - file2del.close(); - } else - espresponse->println((state == SDCARD_NOT_PRESENT) ? "No SD card" : "Busy"); - } else - espresponse->println("Error: Missing file name!"); - } - break; - //print SD file - //[ESP220] - case 220: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if (parameter.length() == 0) { - if (espresponse)espresponse->println("Error: Missing file name!"); - return false; - } - int8_t state = get_sd_state(true); - if (state != SDCARD_IDLE) { - espresponse->println((state == SDCARD_NOT_PRESENT) ? "No SD card" : "Busy"); - return false; - } - if (sys.state != STATE_IDLE) { - if (espresponse)espresponse->println("Busy"); - return false; - } - if (!openFile(SD, parameter.c_str())) { - report_status_message(STATUS_SD_FAILED_READ, (espresponse) ? espresponse->client() : CLIENT_ALL); - espresponse->println(""); - return false; - } - char fileLine[255]; - SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL; - if (!readFileLine(fileLine)) { - //No need notification here it is just a macro - closeFile(); - espresponse->println(""); - return false; - } else { - report_status_message(gc_execute_line(fileLine, (espresponse) ? espresponse->client() : CLIENT_ALL), (espresponse) ? espresponse->client() : CLIENT_ALL); // execute the first line - } - report_realtime_status((espresponse) ? espresponse->client() : CLIENT_ALL); - espresponse->println(""); - } - break; -#endif - //Get full ESP32 settings content - //[ESP400] - case 400: { - String v; - String defV; - Preferences prefs; - if (!espresponse) return false; -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - espresponse->print("{\"EEPROM\":["); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - prefs.begin(NAMESPACE, true); -#ifdef ENABLE_WIFI - int8_t vi; - //1 - Hostname - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(HOSTNAME_ENTRY); - espresponse->print("\",\"T\":\"S\",\"V\":\""); - espresponse->print(wifi_config.Hostname().c_str()); - espresponse->print("\",\"H\":\"Hostname\" ,\"S\":\""); - espresponse->print(String(MAX_HOSTNAME_LENGTH).c_str()); - espresponse->print("\", \"M\":\""); - espresponse->print(String(MIN_HOSTNAME_LENGTH).c_str()); - espresponse->print("\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); -#ifdef ENABLE_HTTP - //2 - http protocol mode - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(HTTP_ENABLE_ENTRY); - espresponse->print("\",\"T\":\"B\",\"V\":\""); - vi = prefs.getChar(HTTP_ENABLE_ENTRY, 1); - espresponse->print(String(vi).c_str()); - espresponse->print("\",\"H\":\"HTTP protocol\",\"O\":[{\"Enabled\":\"1\"},{\"Disabled\":\"0\"}]}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //3 - http port - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(HTTP_PORT_ENTRY); - espresponse->print("\",\"T\":\"I\",\"V\":\""); - espresponse->print(String(web_server.port()).c_str()); - espresponse->print("\",\"H\":\"HTTP Port\",\"S\":\""); - espresponse->print(String(MAX_HTTP_PORT).c_str()); - espresponse->print("\",\"M\":\""); - espresponse->print(String(MIN_HTTP_PORT).c_str()); - espresponse->print("\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); -#endif -#ifdef ENABLE_TELNET - //4 - telnet protocol mode - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(TELNET_ENABLE_ENTRY); - espresponse->print("\",\"T\":\"B\",\"V\":\""); - vi = prefs.getChar(TELNET_ENABLE_ENTRY, DEFAULT_TELNET_STATE); - espresponse->print(String(vi).c_str()); - espresponse->print("\",\"H\":\"Telnet protocol\",\"O\":[{\"Enabled\":\"1\"},{\"Disabled\":\"0\"}]}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //5 - telnet Port - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(TELNET_PORT_ENTRY); - espresponse->print("\",\"T\":\"I\",\"V\":\""); - espresponse->print(String(telnet_server.port()).c_str()); - espresponse->print("\",\"H\":\"Telnet Port\",\"S\":\""); - espresponse->print(String(MAX_TELNET_PORT).c_str()); - espresponse->print("\",\"M\":\""); - espresponse->print(String(MIN_TELNET_PORT).c_str()); - espresponse->print("\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); -#endif - //6 - radio mode - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(ESP_RADIO_MODE); - espresponse->print("\",\"T\":\"B\",\"V\":\""); - vi = prefs.getChar(ESP_RADIO_MODE, ESP_RADIO_OFF); - espresponse->print(String(vi).c_str()); - espresponse->print("\",\"H\":\"Radio mode\",\"O\":[{\"None\":\"0\"}"); -#ifdef ENABLE_WIFI - espresponse->print(",{\"STA\":\"1\"},{\"AP\":\"2\"}"); -#endif -#ifdef ENABLE_BLUETOOTH - espresponse->print(",{\"BT\":\"3\"}"); -#endif - espresponse->print("]}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //7 - STA SSID - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(STA_SSID_ENTRY); - espresponse->print("\",\"T\":\"S\",\"V\":\""); - defV = DEFAULT_STA_SSID; - espresponse->print(prefs.getString(STA_SSID_ENTRY, defV).c_str()); - espresponse->print("\",\"S\":\""); - espresponse->print(String(MAX_SSID_LENGTH).c_str()); - espresponse->print("\",\"H\":\"Station SSID\",\"M\":\""); - espresponse->print(String(MIN_SSID_LENGTH).c_str()); - espresponse->print("\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //8 - STA password - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(STA_PWD_ENTRY); - espresponse->print("\",\"T\":\"S\",\"V\":\""); - espresponse->print(HIDDEN_PASSWORD); - espresponse->print("\",\"S\":\""); - espresponse->print(String(MAX_PASSWORD_LENGTH).c_str()); - espresponse->print("\",\"H\":\"Station Password\",\"M\":\""); - espresponse->print(String(MIN_PASSWORD_LENGTH).c_str()); - espresponse->print("\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - // 9 - STA IP mode - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(STA_IP_MODE_ENTRY); - espresponse->print("\",\"T\":\"B\",\"V\":\""); - espresponse->print(String(prefs.getChar(STA_IP_MODE_ENTRY, DHCP_MODE)).c_str()); - espresponse->print("\",\"H\":\"Station IP Mode\",\"O\":[{\"DHCP\":\"0\"},{\"Static\":\"1\"}]}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //10-STA static IP - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(STA_IP_ENTRY); - espresponse->print("\",\"T\":\"A\",\"V\":\""); - espresponse->print(wifi_config.IP_string_from_int(prefs.getInt(STA_IP_ENTRY, 0)).c_str()); - espresponse->print("\",\"H\":\"Station Static IP\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //11-STA static Gateway - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(STA_GW_ENTRY); - espresponse->print("\",\"T\":\"A\",\"V\":\""); - espresponse->print(wifi_config.IP_string_from_int(prefs.getInt(STA_GW_ENTRY, 0)).c_str()); - espresponse->print("\",\"H\":\"Station Static Gateway\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //12-STA static Mask - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(STA_MK_ENTRY); - espresponse->print("\",\"T\":\"A\",\"V\":\""); - espresponse->print(wifi_config.IP_string_from_int(prefs.getInt(STA_MK_ENTRY, 0)).c_str()); - espresponse->print("\",\"H\":\"Station Static Mask\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //13 - AP SSID - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(AP_SSID_ENTRY); - espresponse->print("\",\"T\":\"S\",\"V\":\""); - defV = DEFAULT_AP_SSID; - espresponse->print(prefs.getString(AP_SSID_ENTRY, defV).c_str()); - espresponse->print("\",\"S\":\""); - espresponse->print(String(MAX_SSID_LENGTH).c_str()); - espresponse->print("\",\"H\":\"AP SSID\",\"M\":\""); - espresponse->print(String(MIN_SSID_LENGTH).c_str()); - espresponse->print("\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //14 - AP password - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(AP_PWD_ENTRY); - espresponse->print("\",\"T\":\"S\",\"V\":\""); - espresponse->print(HIDDEN_PASSWORD); - espresponse->print("\",\"S\":\""); - espresponse->print(String(MAX_PASSWORD_LENGTH).c_str()); - espresponse->print("\",\"H\":\"AP Password\",\"M\":\""); - espresponse->print(String(MIN_PASSWORD_LENGTH).c_str()); - espresponse->print("\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //15 - AP static IP - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(AP_IP_ENTRY); - espresponse->print("\",\"T\":\"A\",\"V\":\""); - defV = DEFAULT_AP_IP; - espresponse->print(wifi_config.IP_string_from_int(prefs.getInt(AP_IP_ENTRY, wifi_config.IP_int_from_string(defV))).c_str()); - espresponse->print("\",\"H\":\"AP Static IP\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //16 - AP Channel - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(AP_CHANNEL_ENTRY); - espresponse->print("\",\"T\":\"B\",\"V\":\""); - espresponse->print(String(prefs.getChar(AP_CHANNEL_ENTRY, DEFAULT_AP_CHANNEL)).c_str()); - espresponse->print("\",\"H\":\"AP Channel\",\"O\":["); - for (int i = MIN_CHANNEL; i <= MAX_CHANNEL ; i++) { - espresponse->print("{\""); - espresponse->print(String(i).c_str()); - espresponse->print("\":\""); - espresponse->print(String(i).c_str()); - espresponse->print("\"}"); - if (i < MAX_CHANNEL) - espresponse->print(","); - } - espresponse->print("]}"); -#ifdef ENABLE_NOTIFICATIONS - espresponse->print(","); - //Notification type - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(NOTIFICATION_TYPE); - espresponse->print("\",\"T\":\"B\",\"V\":\""); - vi = prefs.getChar(NOTIFICATION_TYPE, DEFAULT_NOTIFICATION_TYPE); - espresponse->print(String(vi).c_str()); - espresponse->print("\",\"H\":\"Notification type\",\"O\":[{\"None\":\"0\"}"); - espresponse->print(",{\"Line\":\"3\"},{\"Pushover\":\"1\"}"); - espresponse->print(",{\"Email\":\"2\"}"); - espresponse->print("]}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //Notification token 1 - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(NOTIFICATION_T1); - espresponse->print("\",\"T\":\"S\",\"V\":\""); - defV = DEFAULT_TOKEN; - espresponse->print(prefs.getString(NOTIFICATION_T1, defV).c_str()); - espresponse->print("\",\"S\":\""); - espresponse->print(String(MAX_NOTIFICATION_TOKEN_LENGTH).c_str()); - espresponse->print("\",\"H\":\"Notification Token 1\",\"M\":\""); - espresponse->print(String(MIN_NOTIFICATION_TOKEN_LENGTH).c_str()); - espresponse->print("\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //Notification token 2 - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(NOTIFICATION_T2); - espresponse->print("\",\"T\":\"S\",\"V\":\""); - defV = DEFAULT_TOKEN; - espresponse->print(prefs.getString(NOTIFICATION_T2, defV).c_str()); - espresponse->print("\",\"S\":\""); - espresponse->print(String(MAX_NOTIFICATION_TOKEN_LENGTH).c_str()); - espresponse->print("\",\"H\":\"Notification Token 2\",\"M\":\""); - espresponse->print(String(MIN_NOTIFICATION_TOKEN_LENGTH).c_str()); - espresponse->print("\"}"); - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - //Notification settings - espresponse->print("{\"F\":\"network\",\"P\":\""); - espresponse->print(NOTIFICATION_TS); - espresponse->print("\",\"T\":\"S\",\"V\":\""); - defV = DEFAULT_TOKEN; - espresponse->print(prefs.getString(NOTIFICATION_TS, defV).c_str()); - espresponse->print("\",\"S\":\""); - espresponse->print(String(MAX_NOTIFICATION_SETTING_LENGTH).c_str()); - espresponse->print("\",\"H\":\"Notification Settings\",\"M\":\""); - espresponse->print(String(MIN_NOTIFICATION_TOKEN_LENGTH).c_str()); - espresponse->print("\"}"); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); -#endif //ENABLE_NOTIFICATIONS -#endif - espresponse->print("]}"); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - prefs.end(); - } - break; - //Set EEPROM setting - //[ESP401]P= T= V= pwd= - case 401: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - //check validity of parameters - String spos = get_param(cmd_params, "P=", false); - String styp = get_param(cmd_params, "T=", false); - String sval = get_param(cmd_params, "V=", true); - spos.trim(); - sval.trim(); - if (spos.length() == 0) - response = false; - if (!(styp == "B" || styp == "S" || styp == "A" || styp == "I" || styp == "F")) - response = false; - if ((sval.length() == 0) -#if defined (ENABLE_WIFI) - && !((spos == AP_PWD_ENTRY) || (spos == STA_PWD_ENTRY)) -#endif - ) - response = false; - if (response) { - Preferences prefs; - prefs.begin(NAMESPACE, false); - //Byte value - if ((styp == "B") || (styp == "F")) { - int8_t bbuf = sval.toInt(); - if (prefs.putChar(spos.c_str(), bbuf) == 0) - response = false; - else { -#if defined (ENABLE_WIFI) - //dynamique refresh is better than restart the board - if (spos == ESP_RADIO_MODE) { - //TODO - } - if (spos == AP_CHANNEL_ENTRY) { - //TODO - } -#if defined (ENABLE_HTTP) - if (spos == HTTP_ENABLE_ENTRY) { - //TODO - } -#endif -#if defined (ENABLE_TELNET) - if (spos == TELNET_ENABLE_ENTRY) { - //TODO - } -#endif -#endif - } - } - //Integer value - if (styp == "I") { - int16_t ibuf = sval.toInt(); - if (prefs.putUShort(spos.c_str(), ibuf) == 0) - response = false; - else { -#if defined (ENABLE_WIFI) -#if defined (ENABLE_HTTP) - if (spos == HTTP_PORT_ENTRY) { - //TODO - } -#endif -#if defined (ENABLE_TELNET) - if (spos == TELNET_PORT_ENTRY) { - //TODO - //Serial.println(ibuf); - } -#endif -#endif - } - } - //String value - if (styp == "S") { - if (prefs.putString(spos.c_str(), sval) != sval.length()) - response = false; - else { -#if defined (ENABLE_WIFI) - if (spos == HOSTNAME_ENTRY) { - //TODO - } - if (spos == STA_SSID_ENTRY) { - //TODO - } - if (spos == STA_PWD_ENTRY) { - //TODO - } - if (spos == AP_SSID_ENTRY) { - //TODO - } - if (spos == AP_PWD_ENTRY) { - //TODO - } -#endif - } - } -#if defined (ENABLE_WIFI) - //IP address - if (styp == "A") { - if (prefs.putInt(spos.c_str(), wifi_config.IP_int_from_string(sval)) == 0) - response = false; - else { - if (spos == STA_IP_ENTRY) { - //TODO - } - if (spos == STA_GW_ENTRY) { - //TODO - } - if (spos == STA_MK_ENTRY) { - //TODO - } - if (spos == AP_IP_ENTRY) { - //TODO - } - } - } -#endif - prefs.end(); - } - if (!response) { - if (espresponse) espresponse->println("Error: Incorrect Command"); - } else { - if (espresponse) espresponse->println("ok"); - } - } - break; -#if defined (ENABLE_WIFI) - //Get available AP list (limited to 30) - //output is JSON - //[ESP410] - case 410: { - if (!espresponse)return false; -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - espresponse->print("{\"AP_LIST\":["); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - int n = WiFi.scanComplete(); - if (n == -2) - WiFi.scanNetworks(true); - else if (n) { - for (int i = 0; i < n; ++i) { - if (i > 0) { - espresponse->print(","); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - } - espresponse->print("{\"SSID\":\""); - espresponse->print(WiFi.SSID(i).c_str()); - espresponse->print("\",\"SIGNAL\":\""); - espresponse->print(String(wifi_config.getSignal(WiFi.RSSI(i))).c_str()); - espresponse->print("\",\"IS_PROTECTED\":\""); - if (WiFi.encryptionType(i) == WIFI_AUTH_OPEN) - espresponse->print("0"); - else - espresponse->print("1"); - espresponse->print("\"}"); - } - } - WiFi.scanDelete(); - if (WiFi.scanComplete() == -2) - WiFi.scanNetworks(true); - espresponse->print("]}"); - if (espresponse->client() != CLIENT_WEBUI)espresponse->println(""); - } - break; -#endif - //Get ESP current status - case 420: { -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - if (!espresponse)return false; - espresponse->print("Chip ID: "); - espresponse->print(String((uint16_t)(ESP.getEfuseMac() >> 32)).c_str()); - espresponse->println(""); - espresponse->print("CPU Frequency: "); - espresponse->print(String(ESP.getCpuFreqMHz()).c_str()); - espresponse->print("Mhz"); - espresponse->println(""); - espresponse->print("CPU Temperature: "); - espresponse->print(String(temperatureRead(), 1).c_str()); - if (espresponse->client() == CLIENT_WEBUI)espresponse->print("°"); - espresponse->print("C"); - espresponse->println(""); - espresponse->print("Free memory: "); - espresponse->print(ESPResponseStream::formatBytes(ESP.getFreeHeap()).c_str()); - espresponse->println(""); - espresponse->print("SDK: "); - espresponse->print(ESP.getSdkVersion()); - espresponse->println(""); - espresponse->print("Flash Size: "); - espresponse->print(ESPResponseStream::formatBytes(ESP.getFlashChipSize()).c_str()); - espresponse->println(""); -#if defined (ENABLE_WIFI) - if (WiFi.getMode() != WIFI_MODE_NULL) { - espresponse->print("Available Size for update: "); - //Is OTA available ? - size_t flashsize = 0; - if (esp_ota_get_running_partition()) { - const esp_partition_t* partition = esp_ota_get_next_update_partition(NULL); - if (partition) - flashsize = partition->size; - } - espresponse->print(ESPResponseStream::formatBytes(flashsize).c_str()); - espresponse->println(""); - } - if (WiFi.getMode() != WIFI_MODE_NULL) { - espresponse->print("Available Size for SPIFFS: "); - espresponse->print(ESPResponseStream::formatBytes(SPIFFS.totalBytes()).c_str()); - espresponse->println(""); - } -#endif - espresponse->print("Baud rate: "); - long br = Serial.baudRate(); - //workaround for ESP32 - if (br == 115201) - br = 115200; - if (br == 230423) - br = 230400; - espresponse->print(String(br).c_str()); - espresponse->println(""); - espresponse->print("Sleep mode: "); - if (WiFi.getSleep())espresponse->print("Modem"); - else espresponse->print("None"); - espresponse->println(""); -#if defined (ENABLE_WIFI) -#if defined (ENABLE_HTTP) - if (WiFi.getMode() != WIFI_MODE_NULL) { - espresponse->print("Web port: "); - espresponse->print(String(web_server.port()).c_str()); - espresponse->println(""); - } -#endif -#if defined (ENABLE_TELNET) - if (WiFi.getMode() != WIFI_MODE_NULL) { - espresponse->print("Data port: "); - espresponse->print(String(telnet_server.port()).c_str()); - espresponse->println(""); - } -#endif - if (WiFi.getMode() != WIFI_MODE_NULL) { - espresponse->print("Hostname: "); - espresponse->print(wifi_config.Hostname().c_str()); - espresponse->println(""); - } - espresponse->print("Current WiFi Mode: "); - if (WiFi.getMode() == WIFI_STA) { - espresponse->print("STA ("); - espresponse->print(WiFi.macAddress().c_str()); - espresponse->print(")"); - espresponse->println(""); - espresponse->print("Connected to: "); - if (WiFi.isConnected()) { //in theory no need but ... - espresponse->print(WiFi.SSID().c_str()); - espresponse->println(""); - espresponse->print("Signal: "); - espresponse->print(String(wifi_config.getSignal(WiFi.RSSI())).c_str()); - espresponse->print("%"); - espresponse->println(""); - uint8_t PhyMode; - esp_wifi_get_protocol(ESP_IF_WIFI_STA, &PhyMode); - espresponse->print("Phy Mode: "); - if (PhyMode == (WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N)) espresponse->print("11n"); - else if (PhyMode == (WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G)) espresponse->print("11g"); - else if (PhyMode == (WIFI_PROTOCOL_11B)) espresponse->print("11b"); - else espresponse->print("???"); - espresponse->println(""); - espresponse->print("Channel: "); - espresponse->print(String(WiFi.channel()).c_str()); - espresponse->println(""); - espresponse->print("IP Mode: "); - tcpip_adapter_dhcp_status_t dhcp_status; - tcpip_adapter_dhcpc_get_status(TCPIP_ADAPTER_IF_STA, &dhcp_status); - if (dhcp_status == TCPIP_ADAPTER_DHCP_STARTED)espresponse->print("DHCP"); - else espresponse->print("Static"); - espresponse->println(""); - espresponse->print("IP: "); - espresponse->print(WiFi.localIP().toString().c_str()); - espresponse->println(""); - espresponse->print("Gateway: "); - espresponse->print(WiFi.gatewayIP().toString().c_str()); - espresponse->println(""); - espresponse->print("Mask: "); - espresponse->print(WiFi.subnetMask().toString().c_str()); - espresponse->println(""); - espresponse->print("DNS: "); - espresponse->print(WiFi.dnsIP().toString().c_str()); - espresponse->println(""); - } //this is web command so connection => no command - espresponse->print("Disabled Mode: "); - espresponse->print("AP ("); - espresponse->print(WiFi.softAPmacAddress().c_str()); - espresponse->print(")"); - espresponse->println(""); - } else if (WiFi.getMode() == WIFI_AP) { - espresponse->print("AP ("); - espresponse->print(WiFi.softAPmacAddress().c_str()); - espresponse->print(")"); - espresponse->println(""); - wifi_config_t conf; - esp_wifi_get_config(ESP_IF_WIFI_AP, &conf); - espresponse->print("SSID: "); - espresponse->print((const char*) conf.ap.ssid); - espresponse->println(""); - espresponse->print("Visible: "); - espresponse->print((conf.ap.ssid_hidden == 0) ? "Yes" : "No"); - espresponse->println(""); - espresponse->print("Authentication: "); - if (conf.ap.authmode == WIFI_AUTH_OPEN) - espresponse->print("None"); - else if (conf.ap.authmode == WIFI_AUTH_WEP) - espresponse->print("WEP"); - else if (conf.ap.authmode == WIFI_AUTH_WPA_PSK) - espresponse->print("WPA"); - else if (conf.ap.authmode == WIFI_AUTH_WPA2_PSK) - espresponse->print("WPA2"); - else - espresponse->print("WPA/WPA2"); - espresponse->println(""); - espresponse->print("Max Connections: "); - espresponse->print(String(conf.ap.max_connection).c_str()); - espresponse->println(""); - espresponse->print("DHCP Server: "); - tcpip_adapter_dhcp_status_t dhcp_status; - tcpip_adapter_dhcps_get_status(TCPIP_ADAPTER_IF_AP, &dhcp_status); - if (dhcp_status == TCPIP_ADAPTER_DHCP_STARTED)espresponse->print("Started"); - else espresponse->print("Stopped"); - espresponse->println(""); - espresponse->print("IP: "); - espresponse->print(WiFi.softAPIP().toString().c_str()); - espresponse->println(""); - tcpip_adapter_ip_info_t ip_AP; - tcpip_adapter_get_ip_info(TCPIP_ADAPTER_IF_AP, &ip_AP); - espresponse->print("Gateway: "); - espresponse->print(IPAddress(ip_AP.gw.addr).toString().c_str()); - espresponse->println(""); - espresponse->print("Mask: "); - espresponse->print(IPAddress(ip_AP.netmask.addr).toString().c_str()); - espresponse->println(""); - espresponse->print("Connected clients: "); - wifi_sta_list_t station; - tcpip_adapter_sta_list_t tcpip_sta_list; - esp_wifi_ap_get_sta_list(&station); - tcpip_adapter_get_sta_list(&station, &tcpip_sta_list); - espresponse->print(String(station.num).c_str()); - espresponse->println(""); - for (int i = 0; i < station.num; i++) { - espresponse->print(wifi_config.mac2str(tcpip_sta_list.sta[i].mac)); - espresponse->print(" "); - espresponse->print(IPAddress(tcpip_sta_list.sta[i].ip.addr).toString().c_str()); - espresponse->println(""); - } - espresponse->print("Disabled Mode: "); - espresponse->print("STA ("); - espresponse->print(WiFi.macAddress().c_str()); - espresponse->print(")"); - espresponse->println(""); - } else if (WiFi.getMode() == WIFI_AP_STA) { //we should not be in this state but just in case .... - espresponse->print("Mixed"); - espresponse->println(""); - espresponse->print("STA ("); - espresponse->print(WiFi.macAddress().c_str()); - espresponse->print(")"); - espresponse->println(""); - espresponse->print("AP ("); - espresponse->print(WiFi.softAPmacAddress().c_str()); - espresponse->print(")"); - espresponse->println(""); - } else { //we should not be there if no wifi .... - espresponse->print("Off"); - espresponse->println(""); - } -#endif -#ifdef ENABLE_BLUETOOTH - espresponse->print("Current BT Mode: "); - if (bt_config.Is_BT_on()) { - espresponse->println("On"); - espresponse->print("BT Name: "); - espresponse->print(bt_config.BTname().c_str()); - espresponse->print("("); - espresponse->print(bt_config.device_address()); - espresponse->println(")"); - espresponse->print("Status: "); - if (SerialBT.hasClient()) { - espresponse->print("Connected with "); - espresponse->print(bt_config._btclient.c_str()); - } else espresponse->print("Not connected"); - } else - espresponse->print("Off"); - espresponse->println(""); -#endif -#ifdef ENABLE_NOTIFICATIONS - espresponse->print("Notifications: "); - espresponse->print(notificationsservice.started() ? "Enabled" : "Disabled"); - if (notificationsservice.started()) { - espresponse->print("("); - espresponse->print(notificationsservice.getTypeString()); - espresponse->print(")"); - } - espresponse->println(""); -#endif - //TODO to complete - espresponse->print("FW version: "); - espresponse->print(GRBL_VERSION); - espresponse->print(" ("); - espresponse->print(GRBL_VERSION_BUILD); - espresponse->print(") (ESP32)"); - espresponse->println(""); - } - break; - //Set ESP mode - //cmd is RESTART - //[ESP444] - case 444: - parameter = get_param(cmd_params, "", true); -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - { - if (parameter == "RESTART") { - grbl_send(CLIENT_ALL, "[MSG:Restart ongoing]\r\n"); - COMMANDS::restart_ESP(); - } else response = false; - } - if (!response) { - if (espresponse)espresponse->println("Error: Incorrect Command"); - } else { - if (espresponse)espresponse->println("ok"); - } - break; -#ifdef ENABLE_AUTHENTICATION - //Change / Reset user password - //[ESP555] - case 555: { - if (auth_type == LEVEL_ADMIN) { - parameter = get_param(cmd_params, "", true); - if (parameter.length() == 0) { - Preferences prefs; - parameter = DEFAULT_USER_PWD; - prefs.begin(NAMESPACE, false); - if (prefs.putString(USER_PWD_ENTRY, parameter) != parameter.length()) { - response = false; - if (espresponse)espresponse->println("error"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } else { - if (isLocalPasswordValid(parameter.c_str())) { - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(USER_PWD_ENTRY, parameter) != parameter.length()) { - response = false; - if (espresponse)espresponse->println("error"); - } else if (espresponse)espresponse->println("ok"); - prefs.end(); - } else { - if (espresponse)espresponse->println("error"); - response = false; - } - } - } else { - if (espresponse)espresponse->println("error"); - response = false; - } - break; - } -#endif -#ifdef ENABLE_NOTIFICATIONS - //[ESP600] - case 600: { //Send message -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if (parameter.length() == 0) { - if (espresponse)espresponse->println("Invalid message!"); - return false; - } - if (notificationsservice.sendMSG("GRBL Notification", parameter.c_str())) { - if (espresponse)espresponse->println("ok"); - } else { - if (espresponse)espresponse->println("Cannot send message!"); - return false; - } - } - break; - //Set/Get Notification settings - //[ESP610]type= T1= T2= TS= [pwd=] - //Get will give type and settings only not the protected T1/T2 - case 610: { //Send message -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() == 0) && !espresponse) return false; - //get - if (parameter.length() == 0) { - Preferences prefs; - prefs.begin(NAMESPACE, true); - uint8_t vi = prefs.getChar(NOTIFICATION_TYPE, DEFAULT_NOTIFICATION_TYPE); - String defV = DEFAULT_TOKEN; - parameter = (vi == ESP_PUSHOVER_NOTIFICATION) ? "PUSHOVER" : (vi == ESP_LINE_NOTIFICATION) ? "LINE" : (vi == ESP_EMAIL_NOTIFICATION) ? "EMAIL" : "NONE"; - parameter += " "; - parameter += prefs.getString(NOTIFICATION_TS, defV); - espresponse->println(parameter.c_str()); - prefs.end(); - } else { //set -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - response = false; - parameter = get_param(cmd_params, "type=", false); - if (parameter.length() != 0) { - uint8_t bbuf = (parameter == "NONE") ? 0 : (parameter == "PUSHOVER") ? ESP_PUSHOVER_NOTIFICATION : (parameter == "LINE") ? ESP_LINE_NOTIFICATION : (parameter == "EMAIL") ? ESP_EMAIL_NOTIFICATION : 255; - if (bbuf != 255) { - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putChar(NOTIFICATION_TYPE, bbuf) == 0) { - if (espresponse)espresponse->println("Error: Set failed!"); - response = false; - } else - response = true; - prefs.end(); - } else { - if (espresponse)espresponse->println("Error: wrong type!"); - response = false; - } - } - parameter = get_param(cmd_params, "T1=", false); - if (parameter.length() != 0) { - if (parameter.length() <= MAX_NOTIFICATION_TOKEN_LENGTH) { - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(NOTIFICATION_T1, parameter) == 0) { - if (espresponse)espresponse->println("Error: Set failed!"); - response = false; - } else - response = true; - prefs.end(); - } else { - if (espresponse)espresponse->println("Error: token 1!"); - response = false; - } - } - parameter = get_param(cmd_params, "T2=", false); - if (parameter.length() != 0) { - if (parameter.length() <= MAX_NOTIFICATION_TOKEN_LENGTH) { - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(NOTIFICATION_T2, parameter) == 0) { - if (espresponse)espresponse->println("Error: Set failed!"); - response = false; - } else - response = true; - prefs.end(); - } else { - if (espresponse)espresponse->println("Error: token 2!"); - response = false; - } - } - parameter = get_param(cmd_params, "TS=", false); - if (parameter.length() != 0) { - if (parameter.length() <= MAX_NOTIFICATION_SETTING_LENGTH) { - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(NOTIFICATION_TS, parameter) == 0) { - if (espresponse)espresponse->println("Error: Set failed!"); - response = false; - } else - response = true; - prefs.end(); - } else { - if (espresponse)espresponse->println("Error: settings!"); - response = false; - } - } - } - //update settings - notificationsservice.begin(); - if (espresponse && response)espresponse->println("ok"); - } - break; -#endif //ENABLE_NOTIFICATIONS - //[ESP700] pwd= - case 700: { //read local file -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - if ((parameter.length() > 0) && (parameter[0] != '/')) - parameter = "/" + parameter; - if (!SPIFFS.exists(parameter)) { - if (espresponse)espresponse->println("Error:No such file!"); - response = false; - } else { - File currentfile = SPIFFS.open(parameter, FILE_READ); - if (currentfile) {//if file open success - //until no line in file - while (currentfile.available()) { - String currentline = currentfile.readStringUntil('\n'); - currentline.replace("\n", ""); - currentline.replace("\r", ""); - if (currentline.length() > 0) { - int ESPpos = currentline.indexOf("[ESP"); - if (ESPpos > -1) { - //is there the second part? - int ESPpos2 = currentline.indexOf("]", ESPpos); - if (ESPpos2 > -1) { - //Split in command and parameters - String cmd_part1 = currentline.substring(ESPpos + 4, ESPpos2); - String cmd_part2 = ""; - //is there space for parameters? - if (ESPpos2 < currentline.length()) - cmd_part2 = currentline.substring(ESPpos2 + 1); - //if command is a valid number then execute command - if (cmd_part1.toInt() >= 0) { - if (!execute_internal_command(cmd_part1.toInt(), cmd_part2, auth_type, espresponse)) response = false; - } - //if not is not a valid [ESPXXX] command ignore it - } - } else { - //preprocess line - String processedline = ""; - char c; - uint8_t line_flags = 0; - for (uint16_t index = 0; index < currentline.length(); index++) { - c = currentline[index]; - if (c == '\r' || c == ' ' || c == '\n') { - // ignore these whitespace items - } else if (c == '(') - line_flags |= LINE_FLAG_COMMENT_PARENTHESES; - else if (c == ')') { - // End of '()' comment. Resume line allowed. - if (line_flags & LINE_FLAG_COMMENT_PARENTHESES) line_flags &= ~(LINE_FLAG_COMMENT_PARENTHESES); - } else if (c == ';') { - // NOTE: ';' comment to EOL is a LinuxCNC definition. Not NIST. - if (!(line_flags & LINE_FLAG_COMMENT_PARENTHESES)) // semi colon inside parentheses do not mean anything - line_flags |= LINE_FLAG_COMMENT_SEMICOLON; - } else { // add characters to the line - if (!line_flags) { - c = toupper(c); // make upper case - processedline += c; - } - } - } - if (processedline.length() > 0)gc_execute_line((char*)processedline.c_str(), CLIENT_WEBUI); - wait(1); - } - wait(1); - } - } - currentfile.close(); - if (espresponse)espresponse->println("ok"); - } else { - if (espresponse)espresponse->println("error"); - response = false; - } - } - break; - } - //Format SPIFFS - //[ESP710]FORMAT pwd= - case 710: -#ifdef ENABLE_AUTHENTICATION - if (auth_type != LEVEL_ADMIN) { - if (espresponse)espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - { - if (parameter == "FORMAT") { - if (espresponse)espresponse->print("Formating"); - SPIFFS.format(); - if (espresponse)espresponse->println("...Done"); - } else { - if (espresponse)espresponse->println("error"); - response = false; - } - } - break; - //SPIFFS total size and used size - //[ESP720]
- case 720: - if (!espresponse)return false; -#ifdef ENABLE_AUTHENTICATION - if (auth_type == LEVEL_GUEST) { - espresponse->println("Error: Wrong authentication!"); - return false; - } -#endif - parameter = get_param(cmd_params, "", true); - espresponse->print(parameter.c_str()); - espresponse->print("SPIFFS Total:"); - espresponse->print(ESPResponseStream::formatBytes(SPIFFS.totalBytes()).c_str()); - espresponse->print(" Used:"); - espresponse->println(ESPResponseStream::formatBytes(SPIFFS.usedBytes()).c_str()); - break; - //get fw version / fw target / hostname / authentication - //[ESP800] - case 800: { - if (!espresponse)return false; - String resp; - resp = "FW version:"; - resp += GRBL_VERSION ; - resp += " ("; - resp += GRBL_VERSION_BUILD; - resp += ")"; - resp += " # FW target:grbl-embedded # FW HW:"; -#ifdef ENABLE_SD_CARD - resp += "Direct SD"; -#else - resp += "No SD"; -#endif - resp += " # primary sd:/sd # secondary sd:none # authentication:"; -#ifdef ENABLE_AUTHENTICATION - resp += "yes"; -#else - resp += "no"; -#endif -#if defined (ENABLE_WIFI) -#if defined (ENABLE_HTTP) - resp += " # webcommunication: Sync: "; - resp += String(web_server.port() + 1); - resp += ":"; - if (WiFi.getMode() == WIFI_MODE_AP) - resp += WiFi.softAPIP().toString(); - else if (WiFi.getMode() == WIFI_MODE_STA) - resp += WiFi.localIP().toString(); - else if (WiFi.getMode() == WIFI_MODE_APSTA) - resp += WiFi.softAPIP().toString(); - else - resp += "0.0.0.0"; -#endif - resp += " # hostname:"; - resp += wifi_config.Hostname(); - if (WiFi.getMode() == WIFI_AP)resp += "(AP mode)"; -#endif - //to save time in decoding `?` - resp += " # axis:"; - resp += String(N_AXIS); - if (espresponse)espresponse->println(resp.c_str()); - } - break; - default: - if (espresponse)espresponse->println("Error: Incorrect Command"); - response = false; - break; - } - return response; -} - - -String COMMANDS::get_param(String& cmd_params, const char* id, bool withspace) { - static String parameter; - String sid = id; - int start; - int end = -1; - if (cmd_params.indexOf("pwd=") == 0)cmd_params = " " + cmd_params; - parameter = ""; - //if no id it means it is first part of cmd - if (strlen(id) == 0) - start = 0; - //else find id position - else - start = cmd_params.indexOf(id); - //if no id found and not first part leave - if (start == -1) - return parameter; - //password and SSID can have space so handle it - //if no space expected use space as delimiter - if (!withspace) - end = cmd_params.indexOf(" ", start); -#ifdef ENABLE_AUTHENTICATION - //if space expected only one parameter but additional password may be present - else if (sid != " pwd=") - end = cmd_params.indexOf(" pwd=", start); -#endif - //if no end found - take all - if (end == -1) - end = cmd_params.length(); - //extract parameter - parameter = cmd_params.substring(start + strlen(id), end); - //be sure no extra space - parameter.trim(); - return parameter; -} - -#ifdef ENABLE_AUTHENTICATION - -bool COMMANDS::isLocalPasswordValid(const char* password) { +bool COMMANDS::isLocalPasswordValid(char* password) { char c; //limited size if ((strlen(password) > MAX_LOCAL_PASSWORD_LENGTH) || (strlen(password) < MIN_LOCAL_PASSWORD_LENGTH)) @@ -2061,68 +54,6 @@ bool COMMANDS::isLocalPasswordValid(const char* password) { return true; } -//check admin password -bool COMMANDS::isadmin(String& cmd_params) { - String adminpassword; - String sadminPassword; - Preferences prefs; - prefs.begin(NAMESPACE, true); - String defV = DEFAULT_ADMIN_PWD; - sadminPassword = prefs.getString(ADMIN_PWD_ENTRY, defV); - prefs.end(); - adminpassword = get_param(cmd_params, "pwd=", true); - if (!sadminPassword.equals(adminpassword)) - return false; - else - return true; -} -//check user password - admin password is also valid -bool COMMANDS::isuser(String& cmd_params) { - String userpassword; - String suserPassword; - Preferences prefs; - prefs.begin(NAMESPACE, true); - String defV = DEFAULT_USER_PWD; - suserPassword = prefs.getString(USER_PWD_ENTRY, defV); - prefs.end(); - userpassword = get_param(cmd_params, "pwd=", true); - //it is not user password - if (!suserPassword.equals(userpassword)) { - //check admin password - return isadmin(cmd_params); - } else - return true; -} -#endif - -//check is valid [ESPXXX] command -//return XXX as cmd and command as cmd_params -bool COMMANDS::check_command(const char* line, int* cmd, String& cmd_params) { - String buffer = line; - bool result = false; - int ESPpos = buffer.indexOf("[ESP"); - if (ESPpos > -1) { - //is there the second part? - int ESPpos2 = buffer.indexOf("]", ESPpos); - if (ESPpos2 > -1) { - //Split in command and parameters - String cmd_part1 = buffer.substring(ESPpos + 4, ESPpos2); - String cmd_part2 = ""; - //is there space for parameters? - if (ESPpos2 < buffer.length()) - cmd_part2 = buffer.substring(ESPpos2 + 1); - //if command is a valid number then execute command - if (cmd_part1.toInt() >= 0) { - *cmd = cmd_part1.toInt(); - cmd_params = cmd_part2; - result = true; - } - //if not is not a valid [ESPXXX] command - } - } - return result; -} - /** * Restart ESP */ diff --git a/Grbl_Esp32/commands.h b/Grbl_Esp32/commands.h index 4823bc4d..544ad9ef 100644 --- a/Grbl_Esp32/commands.h +++ b/Grbl_Esp32/commands.h @@ -22,34 +22,14 @@ #define COMMANDS_h #include "config.h" -//Authentication level -typedef enum { - LEVEL_GUEST = 0, - LEVEL_USER = 1, - LEVEL_ADMIN = 2 -} level_authenticate_type; - -// Define line flags. Includes comment type tracking and line overflow detection. -#define LINE_FLAG_OVERFLOW bit(0) -#define LINE_FLAG_COMMENT_PARENTHESES bit(1) -#define LINE_FLAG_COMMENT_SEMICOLON bit(2) - class ESPResponseStream; - class COMMANDS { public: - static bool check_command(const char*, int* cmd, String& cmd_params); - static String get_param(String& cmd_params, const char* id, bool withspace); - static bool execute_internal_command(int cmd, String cmd_params, level_authenticate_type auth_level = LEVEL_GUEST, ESPResponseStream* espresponse = NULL); static void wait(uint32_t milliseconds); static void handle(); static void restart_ESP(); -#ifdef ENABLE_AUTHENTICATION - static bool isadmin(String& cmd_params); - static bool isuser(String& cmd_params); - static bool isLocalPasswordValid(const char* password); -#endif + static bool isLocalPasswordValid(char* password); private : static bool restart_ESP_module; }; diff --git a/Grbl_Esp32/config.h b/Grbl_Esp32/config.h index e625a1bd..e50f2103 100644 --- a/Grbl_Esp32/config.h +++ b/Grbl_Esp32/config.h @@ -62,13 +62,13 @@ Some features should not be changed. See notes below. // NOTE: Defaults are set for a traditional 3-axis CNC machine. Z-axis first to clear, followed by X & Y. // These homing cycle definitions precede the machine.h file so that the machine // definition can undefine them if necessary. -#define HOMING_CYCLE_0 (1< #endif -#include "report.h" - #if defined (ENABLE_HTTP) && defined(ENABLE_WIFI) ESPResponseStream::ESPResponseStream(WebServer* webserver) { _header_sent = false; diff --git a/Grbl_Esp32/espresponse.h b/Grbl_Esp32/espresponse.h index 7f6bf56c..3986a67f 100644 --- a/Grbl_Esp32/espresponse.h +++ b/Grbl_Esp32/espresponse.h @@ -20,7 +20,6 @@ #ifndef ESPRESPONSE_h #define ESPRESPONSE_h -#include "config.h" #if defined (ENABLE_HTTP) && defined(ENABLE_WIFI) class WebServer; @@ -31,6 +30,7 @@ class ESPResponseStream { void print(const char* data); void println(const char* data); void flush(); + bool anyOutput() { return _header_sent; } static String formatBytes(uint64_t bytes); uint8_t client() {return _client;} #if defined (ENABLE_HTTP) && defined(ENABLE_WIFI) @@ -40,8 +40,8 @@ class ESPResponseStream { ESPResponseStream(); private: uint8_t _client; -#if defined (ENABLE_HTTP) && defined(ENABLE_WIFI) bool _header_sent; +#if defined (ENABLE_HTTP) && defined(ENABLE_WIFI) WebServer* _webserver; String _buffer; #endif diff --git a/Grbl_Esp32/gcode.cpp b/Grbl_Esp32/gcode.cpp index bda0960d..0fd5eb75 100644 --- a/Grbl_Esp32/gcode.cpp +++ b/Grbl_Esp32/gcode.cpp @@ -57,12 +57,78 @@ void gc_sync_position() { } -// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase -// characters and signed floating point values (no whitespace). Comments and block delete -// characters have been removed. In this function, all units and positions are converted and +// Edit GCode line in-place, removing whitespace and comments and +// converting to uppercase +void collapseGCode(char *line) { + // parenPtr, if non-NULL, is the address of the character after ( + char* parenPtr = NULL; + // outPtr is the address where newly-processed characters will be placed. + // outPtr is alway less than or equal to inPtr. + char* outPtr = line; + char c; + for (char* inPtr = line; (c = *inPtr) != '\0'; inPtr++) { + if (isspace(c)) { + continue; + } + switch (c) { + case ')': + if (parenPtr) { + // Terminate comment by replacing ) with NUL + *inPtr = '\0'; + report_gcode_comment(parenPtr); + parenPtr = NULL; + } + // Strip out ) that does not follow a ( + break; + case '(': + // Start the comment at the character after ( + parenPtr = inPtr + 1; + break; + case ';': + // NOTE: ';' comment to EOL is a LinuxCNC definition. Not NIST. +#ifdef REPORT_SEMICOLON_COMMENTS + report_gcode_comment(inPtr + 1); +#endif + *outPtr = '\0'; + return; + case '%': + // TODO: Install '%' feature + // Program start-end percent sign NOT SUPPORTED. + // NOTE: This maybe installed to tell Grbl when a program is running vs manual input, + // where, during a program, the system auto-cycle start will continue to execute + // everything until the next '%' sign. This will help fix resuming issues with certain + // functions that empty the planner buffer to execute its task on-time. + break; + case '\r': + // In case one sneaks in + break; + default: + if (!parenPtr) { + *outPtr++ = toupper(c); // make upper case + } + } + } + // On loop exit, *inPtr is '\0' + if (parenPtr) { + // Handle unterminated ( comments + report_gcode_comment(parenPtr); + } + *outPtr = '\0'; +} + +// Executes one line of NUL-terminated G-Code. +// The line may contain whitespace and comments, which are first removed, +// and lower case characters, which are converted to upper case. +// In this function, all units and positions are converted and // exported to grbl's internal functions in terms of (mm, mm/min) and absolute machine // coordinates, respectively. uint8_t gc_execute_line(char* line, uint8_t client) { + // Step 0 - remove whitespace and comments and convert to upper case + collapseGCode(line); +#ifdef REPORT_ECHO_LINE_RECEIVED + report_echo_line_received(line, client); +#endif + /* ------------------------------------------------------------------------------------- STEP 1: Initialize parser block struct and copy current g-code state modes. The parser updates these modes and commands as the block line is parser and will only be used and @@ -301,7 +367,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) { gc_block.modal.spindle = SPINDLE_ENABLE_CW; break; case 4: // Supported if SPINDLE_DIR_PIN is defined or laser mode is on. - if (spindle->is_reversable || bit_istrue(settings.flags, BITFLAG_LASER_MODE)) + if (spindle->is_reversable || laser_mode->get()) gc_block.modal.spindle = SPINDLE_ENABLE_CCW; else FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); @@ -378,21 +444,21 @@ uint8_t gc_execute_line(char* line, uint8_t client) { case 'A': word_bit = WORD_A; gc_block.values.xyz[A_AXIS] = value; - axis_words |= (1 << A_AXIS); + axis_words |= bit(A_AXIS); break; #endif #if (N_AXIS > B_AXIS) case 'B': word_bit = WORD_B; gc_block.values.xyz[B_AXIS] = value; - axis_words |= (1 << B_AXIS); + axis_words |= bit(B_AXIS); break; #endif #if (N_AXIS > C_AXIS) case 'C': word_bit = WORD_C; gc_block.values.xyz[C_AXIS] = value; - axis_words |= (1 << C_AXIS); + axis_words |= bit(C_AXIS); break; #endif // case 'D': // Not supported @@ -404,17 +470,17 @@ uint8_t gc_execute_line(char* line, uint8_t client) { case 'I': word_bit = WORD_I; gc_block.values.ijk[X_AXIS] = value; - ijk_words |= (1 << X_AXIS); + ijk_words |= bit(X_AXIS); break; case 'J': word_bit = WORD_J; gc_block.values.ijk[Y_AXIS] = value; - ijk_words |= (1 << Y_AXIS); + ijk_words |= bit(Y_AXIS); break; case 'K': word_bit = WORD_K; gc_block.values.ijk[Z_AXIS] = value; - ijk_words |= (1 << Z_AXIS); + ijk_words |= bit(Z_AXIS); break; case 'L': word_bit = WORD_L; @@ -448,17 +514,17 @@ uint8_t gc_execute_line(char* line, uint8_t client) { case 'X': word_bit = WORD_X; gc_block.values.xyz[X_AXIS] = value; - axis_words |= (1 << X_AXIS); + axis_words |= bit(X_AXIS); break; case 'Y': word_bit = WORD_Y; gc_block.values.xyz[Y_AXIS] = value; - axis_words |= (1 << Y_AXIS); + axis_words |= bit(Y_AXIS); break; case 'Z': word_bit = WORD_Z; gc_block.values.xyz[Z_AXIS] = value; - axis_words |= (1 << Z_AXIS); + axis_words |= bit(Z_AXIS); break; default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); @@ -635,7 +701,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) { // is absent or if any of the other axis words are present. if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET) { // Indicates called in block. if (gc_block.modal.tool_length == TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC) { - if (axis_words ^ (1 << TOOL_LENGTH_OFFSET_AXIS)) + if (axis_words ^ bit(TOOL_LENGTH_OFFSET_AXIS)) FAIL(STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR); } } @@ -671,7 +737,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) { if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS) }; // [No axis words] - if (bit_isfalse(value_words, ((1 << WORD_P) | (1 << WORD_L)))) { + if (bit_isfalse(value_words, (bit(WORD_P) | bit(WORD_L)))) { FAIL(STATUS_GCODE_VALUE_WORD_MISSING); // [P/L word missing] } coord_select = trunc(gc_block.values.p); // Convert p value to int. @@ -776,7 +842,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) { if (axis_words) { // Move only the axes specified in secondary move. for (idx = 0; idx < N_AXIS; idx++) { - if (!(axis_words & (1 << idx))) + if (!(axis_words & bit(idx))) gc_block.values.ijk[idx] = gc_state.position[idx]; } } else { @@ -1037,7 +1103,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) { return (status); } // If in laser mode, setup laser power based on current and past parser conditions. - if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) { + if (laser_mode->get()) { if (!((gc_block.modal.motion == MOTION_MODE_LINEAR) || (gc_block.modal.motion == MOTION_MODE_CW_ARC) || (gc_block.modal.motion == MOTION_MODE_CCW_ARC))) gc_parser_flags |= GC_PARSER_LASER_DISABLE; @@ -1125,7 +1191,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) { // turn on/off an i/o pin if ((gc_block.modal.io_control == NON_MODAL_IO_ENABLE) || (gc_block.modal.io_control == NON_MODAL_IO_DISABLE)) { if (gc_block.values.p <= MAX_USER_DIGITAL_PIN) - sys_io_control(1 << (int)gc_block.values.p, (gc_block.modal.io_control == NON_MODAL_IO_ENABLE)); + sys_io_control(bit((int)gc_block.values.p), (gc_block.modal.io_control == NON_MODAL_IO_ENABLE)); else FAIL(STATUS_P_PARAM_MAX_EXCEEDED); } diff --git a/Grbl_Esp32/grbl.h b/Grbl_Esp32/grbl.h index b4292c95..c179f273 100644 --- a/Grbl_Esp32/grbl.h +++ b/Grbl_Esp32/grbl.h @@ -21,8 +21,8 @@ #pragma once // Grbl versioning system -#define GRBL_VERSION "1.2a" -#define GRBL_VERSION_BUILD "20200710" +#define GRBL_VERSION "1.3a" +#define GRBL_VERSION_BUILD "20200720" //#include #include @@ -41,6 +41,7 @@ #include "defaults.h" #include "settings.h" +#include "authentication.h" #include "system.h" #include "planner.h" @@ -54,19 +55,24 @@ #include "protocol.h" #include "report.h" #include "serial.h" +#include "Pins.h" #include "Spindles/SpindleClass.h" +#include "Motors/MotorClass.h" #include "stepper.h" #include "jog.h" #include "inputbuffer.h" +#include "commands.h" +#include "SettingsClass.h" +#include "SettingsDefinitions.h" +#include "WebSettings.h" + +// Do not guard this because it is needed for local files too +#include "grbl_sd.h" #ifdef ENABLE_BLUETOOTH #include "BTconfig.h" #endif -#ifdef ENABLE_SD_CARD - #include "grbl_sd.h" -#endif - #ifdef ENABLE_WIFI #include "wificonfig.h" #ifdef ENABLE_HTTP @@ -94,6 +100,10 @@ #include "grbl_unipolar.h" #endif +#ifdef USE_I2S_OUT + #include "i2s_out.h" +#endif + // Called if USE_MACHINE_INIT is defined void machine_init(); diff --git a/Grbl_Esp32/grbl_eeprom.cpp b/Grbl_Esp32/grbl_eeprom.cpp index 585b8df9..658fce07 100644 --- a/Grbl_Esp32/grbl_eeprom.cpp +++ b/Grbl_Esp32/grbl_eeprom.cpp @@ -20,7 +20,7 @@ #include "grbl.h" -void memcpy_to_eeprom_with_checksum(unsigned int destination, char* source, unsigned int size) { +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); diff --git a/Grbl_Esp32/grbl_eeprom.h b/Grbl_Esp32/grbl_eeprom.h index 9780df5e..1c9dcf77 100644 --- a/Grbl_Esp32/grbl_eeprom.h +++ b/Grbl_Esp32/grbl_eeprom.h @@ -25,7 +25,7 @@ //unsigned char eeprom_get_char(unsigned int addr); //void eeprom_put_char(unsigned int addr, unsigned char new_value); -void memcpy_to_eeprom_with_checksum(unsigned int destination, char* source, unsigned int size); +void memcpy_to_eeprom_with_checksum(unsigned int destination, const char* source, unsigned int size); int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, unsigned int size); #endif diff --git a/Grbl_Esp32/grbl_limits.cpp b/Grbl_Esp32/grbl_limits.cpp index ec418a04..abddc9ba 100644 --- a/Grbl_Esp32/grbl_limits.cpp +++ b/Grbl_Esp32/grbl_limits.cpp @@ -77,6 +77,7 @@ void IRAM_ATTR isr_limit_switches() { void limits_go_home(uint8_t cycle_mask) { if (sys.abort) return; // Block if system reset has been issued. // Initialize plan data struct for homing motion. Spindle and coolant are disabled. + motors_set_homing_mode(cycle_mask, true); // tell motors homing is about to start plan_line_data_t plan_data; plan_line_data_t* pl_data = &plan_data; memset(pl_data, 0, sizeof(plan_line_data_t)); @@ -98,13 +99,12 @@ void limits_go_home(uint8_t cycle_mask) { #endif if (bit_istrue(cycle_mask, bit(idx))) { // Set target based on max_travel setting. Ensure homing switches engaged with search scalar. - // NOTE: settings.max_travel[] is stored as a negative value. - max_travel = MAX(max_travel, (-HOMING_AXIS_SEARCH_SCALAR) * settings.max_travel[idx]); + max_travel = MAX(max_travel, (HOMING_AXIS_SEARCH_SCALAR) * axis_settings[idx]->max_travel->get()); } } // Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches. bool approach = true; - float homing_rate = settings.homing_seek_rate; + float homing_rate = homing_seek_rate->get(); uint8_t limit_state, axislock, n_active_axis; do { system_convert_array_steps_to_mpos(target, sys_position); @@ -130,7 +130,8 @@ void limits_go_home(uint8_t cycle_mask) { #endif // Set target direction based on cycle mask and homing cycle approach state. // NOTE: This happens to compile smaller than any other implementation tried. - if (bit_istrue(settings.homing_dir_mask, bit(idx))) { + auto mask = homing_dir_mask->get(); + if (bit_istrue(mask, bit(idx))) { if (approach) target[idx] = -max_travel; else target[idx] = max_travel; } else { @@ -155,7 +156,7 @@ void limits_go_home(uint8_t cycle_mask) { limit_state = limits_get_state(); for (idx = 0; idx < N_AXIS; idx++) { if (axislock & step_pin[idx]) { - if (limit_state & (1 << idx)) { + if (limit_state & bit(idx)) { #ifdef COREXY if (idx == Z_AXIS) axislock &= ~(step_pin[Z_AXIS]); else axislock &= ~(step_pin[A_MOTOR] | step_pin[B_MOTOR]); @@ -179,7 +180,8 @@ void limits_go_home(uint8_t cycle_mask) { if (!approach && (limits_get_state() & cycle_mask)) system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_PULLOFF); // Homing failure condition: Limit switch not found during approach. if (approach && (rt_exec & EXEC_CYCLE_STOP)) system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_APPROACH); - if (sys_rt_exec_alarm) { + if (sys_rt_exec_alarm) { + motors_set_homing_mode(cycle_mask, false); // tell motors homing is done...failed mc_reset(); // Stop motors, if they are running. protocol_execute_realtime(); return; @@ -190,17 +192,22 @@ void limits_go_home(uint8_t cycle_mask) { } } } while (STEP_MASK & axislock); +#ifdef USE_I2S_OUT_STREAM + if (!approach) { + delay_ms(I2S_OUT_DELAY_MS); + } +#endif st_reset(); // Immediately force kill steppers and reset step segment buffer. - delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate. + delay_ms(homing_debounce->get()); // Delay to allow transient dynamics to dissipate. // Reverse direction and reset homing rate for locate cycle(s). approach = !approach; // After first cycle, homing enters locating phase. Shorten search to pull-off distance. if (approach) { - max_travel = settings.homing_pulloff * HOMING_AXIS_LOCATE_SCALAR; - homing_rate = settings.homing_feed_rate; + max_travel = homing_pulloff->get() * HOMING_AXIS_LOCATE_SCALAR; + homing_rate = homing_feed_rate->get(); } else { - max_travel = settings.homing_pulloff; - homing_rate = settings.homing_seek_rate; + max_travel = homing_pulloff->get(); + homing_rate = homing_seek_rate->get(); } } while (n_cycle-- > 0); // The active cycle axes should now be homed and machine limits have been located. By @@ -211,23 +218,26 @@ void limits_go_home(uint8_t cycle_mask) { // triggering when hard limits are enabled or when more than one axes shares a limit pin. int32_t set_axis_position; // Set machine positions for homed limit switches. Don't update non-homed axes. + auto mask = homing_dir_mask->get(); + auto pulloff = homing_pulloff->get(); for (idx = 0; idx < N_AXIS; idx++) { - // NOTE: settings.max_travel[] is stored as a negative value. + auto steps = axis_settings[idx]->steps_per_mm->get(); if (cycle_mask & bit(idx)) { #ifdef HOMING_FORCE_SET_ORIGIN set_axis_position = 0; #else - if (bit_istrue(settings.homing_dir_mask, bit(idx))) { + auto travel = axis_settings[idx]->max_travel->get(); + if (bit_istrue(mask, bit(idx))) { #ifdef HOMING_FORCE_POSITIVE_SPACE set_axis_position = 0; //lround(settings.homing_pulloff*settings.steps_per_mm[idx]); #else - set_axis_position = lround((settings.max_travel[idx] + settings.homing_pulloff) * settings.steps_per_mm[idx]); + set_axis_position = lround((-travel + pulloff) * steps); #endif } else { #ifdef HOMING_FORCE_POSITIVE_SPACE - set_axis_position = lround((-settings.max_travel[idx] - settings.homing_pulloff) * settings.steps_per_mm[idx]); + set_axis_position = lround((-travel - pulloff) * steps); #else - set_axis_position = lround(-settings.homing_pulloff * settings.steps_per_mm[idx]); + set_axis_position = lround(-pulloff * steps); #endif } #endif @@ -248,71 +258,39 @@ void limits_go_home(uint8_t cycle_mask) { } } sys.step_control = STEP_CONTROL_NORMAL_OP; // Return step control to normal operation. + motors_set_homing_mode(cycle_mask, false); // tell motors homing is done } +uint8_t limit_pins[] = { + X_LIMIT_PIN, + Y_LIMIT_PIN, + Z_LIMIT_PIN, + A_LIMIT_PIN, + B_LIMIT_PIN, + C_LIMIT_PIN, +}; + +uint8_t limit_mask = 0; void limits_init() { -#ifndef DISABLE_LIMIT_PIN_PULL_UP -#ifdef X_LIMIT_PIN - pinMode(X_LIMIT_PIN, INPUT_PULLUP); // input with pullup + limit_mask = 0; + int mode = INPUT_PULLUP; +#ifdef DISABLE_LIMIT_PIN_PULL_UP + mode = INPUT; #endif -#ifdef Y_LIMIT_PIN - pinMode(Y_LIMIT_PIN, INPUT_PULLUP); -#endif -#ifdef Z_LIMIT_PIN - pinMode(Z_LIMIT_PIN, INPUT_PULLUP); -#endif -#ifdef A_LIMIT_PIN - pinMode(A_LIMIT_PIN, INPUT_PULLUP); -#endif -#ifdef B_LIMIT_PIN - pinMode(B_LIMIT_PIN, INPUT_PULLUP); -#endif -#ifdef C_LIMIT_PIN - pinMode(C_LIMIT_PIN, INPUT_PULLUP); -#endif -#else -#ifdef X_LIMIT_PIN - pinMode(X_LIMIT_PIN, INPUT); // input no pullup -#endif -#ifdef Y_LIMIT_PIN - pinMode(Y_LIMIT_PIN, INPUT); -#endif -#ifdef Z_LIMIT_PIN - pinMode(Z_LIMIT_PIN, INPUT); -#endif -#ifdef A_LIMIT_PIN - pinMode(A_LIMIT_PIN, INPUT); // input no pullup -#endif -#ifdef B_LIMIT_PIN - pinMode(B_LIMIT_PIN, INPUT); -#endif -#ifdef C_LIMIT_PIN - pinMode(C_LIMIT_PIN, INPUT); -#endif -#endif - if (bit_istrue(settings.flags, BITFLAG_HARD_LIMIT_ENABLE)) { - // attach interrupt to them -#ifdef X_LIMIT_PIN - attachInterrupt(digitalPinToInterrupt(X_LIMIT_PIN), isr_limit_switches, CHANGE); -#endif -#ifdef Y_LIMIT_PIN - attachInterrupt(digitalPinToInterrupt(Y_LIMIT_PIN), isr_limit_switches, CHANGE); -#endif -#ifdef Z_LIMIT_PIN - attachInterrupt(digitalPinToInterrupt(Z_LIMIT_PIN), isr_limit_switches, CHANGE); -#endif -#ifdef A_LIMIT_PIN - attachInterrupt(digitalPinToInterrupt(A_LIMIT_PIN), isr_limit_switches, CHANGE); -#endif -#ifdef B_LIMIT_PIN - attachInterrupt(digitalPinToInterrupt(B_LIMIT_PIN), isr_limit_switches, CHANGE); -#endif -#ifdef C_LIMIT_PIN - attachInterrupt(digitalPinToInterrupt(C_LIMIT_PIN), isr_limit_switches, CHANGE); -#endif - } else - limits_disable(); + for (int i=0; iget()) { + attachInterrupt(pin, isr_limit_switches, CHANGE); + } else { + detachInterrupt(pin); + } + } + } + // setup task used for debouncing limit_sw_queue = xQueueCreate(10, sizeof(int)); xTaskCreate(limitCheckTask, @@ -323,55 +301,34 @@ void limits_init() { NULL); } - // Disables hard limits. void limits_disable() { - detachInterrupt(X_LIMIT_BIT); - detachInterrupt(Y_LIMIT_BIT); - detachInterrupt(Z_LIMIT_BIT); - detachInterrupt(A_LIMIT_BIT); - detachInterrupt(B_LIMIT_BIT); - detachInterrupt(C_LIMIT_BIT); + for (int i=0; iget()) { + pinMask ^= limit_mask; + } + return (pinMask); } // Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed, @@ -380,11 +337,11 @@ uint8_t limits_get_state() { void limits_soft_check(float* target) { if (system_check_travel_limits(target)) { // TODO for debugging only 3 axes right now - grbl_msg_sendf(CLIENT_SERIAL, - MSG_LEVEL_INFO, - "Soft limit error target WPOS X:%5.2f Y:%5.2f Z:%5.2f", + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "Soft limit error target WPOS X:%5.2f Y:%5.2f Z:%5.2f", target[X_AXIS] - gc_state.coord_system[X_AXIS], - target[Y_AXIS] - gc_state.coord_system[Y_AXIS], + target[Y_AXIS] - gc_state.coord_system[Y_AXIS], target[Z_AXIS] - gc_state.coord_system[Z_AXIS]); sys.soft_limit = true; @@ -439,7 +396,22 @@ bool axis_is_squared(uint8_t axis_mask) { if (axis_mask == (1 << Z_AXIS)) { #ifdef Z_AXIS_SQUARING return true; +#endif + } + if (axis_mask == (1 << A_AXIS)) { +#ifdef A_AXIS_SQUARING + return true; +#endif + } + if (axis_mask == (1 << B_AXIS)) { +#ifdef B_AXIS_SQUARING + return true; +#endif + } + if (axis_mask == (1 << C_AXIS)) { +#ifdef C_AXIS_SQUARING + return true; #endif } return false; -} \ No newline at end of file +} diff --git a/Grbl_Esp32/grbl_sd.cpp b/Grbl_Esp32/grbl_sd.cpp index a0bbc628..0b0a9bc8 100644 --- a/Grbl_Esp32/grbl_sd.cpp +++ b/Grbl_Esp32/grbl_sd.cpp @@ -20,11 +20,6 @@ #include "grbl_sd.h" -// Define line flags. Includes comment type tracking and line overflow detection. -#define LINE_FLAG_OVERFLOW bit(0) -#define LINE_FLAG_COMMENT_PARENTHESES bit(1) -#define LINE_FLAG_COMMENT_SEMICOLON bit(2) - File myFile; bool SD_ready_next = false; // Grbl has processed a line and is waiting for another uint8_t SD_client = CLIENT_SERIAL; @@ -86,65 +81,31 @@ boolean closeFile() { } /* - read a line from the SD card - strip whitespace - strip comments per http://linuxcnc.org/docs/ja/html/gcode/overview.html#gcode:comments - make uppercase - return true if a line is + read a line from the SD card + strip whitespace + strip comments per http://linuxcnc.org/docs/ja/html/gcode/overview.html#gcode:comments + make uppercase + return true if a line is */ -boolean readFileLine(char* line) { - char c; - uint8_t index = 0; - uint8_t line_flags = 0; - uint8_t comment_char_counter = 0; +boolean readFileLine(char* line, int maxlen) { if (!myFile) { report_status_message(STATUS_SD_FAILED_READ, SD_client); return false; } sd_current_line_number += 1; + int len = 0; while (myFile.available()) { - c = myFile.read(); - if (line_flags & LINE_FLAG_COMMENT_PARENTHESES) // capture all characters into a comment buffer - comment[comment_char_counter++] = c; - if (c == '\r' || c == ' ') { - // ignore these whitespace items - } else if (c == '(') - line_flags |= LINE_FLAG_COMMENT_PARENTHESES; - else if (c == ')') { - // End of '()' comment. Resume line allowed. - if (line_flags & LINE_FLAG_COMMENT_PARENTHESES) { - line_flags &= ~(LINE_FLAG_COMMENT_PARENTHESES); - comment[comment_char_counter] = 0; // null terminate - report_gcode_comment(comment); - } - } else if (c == ';') { - // NOTE: ';' comment to EOL is a LinuxCNC definition. Not NIST. - if (!(line_flags & LINE_FLAG_COMMENT_PARENTHESES)) // semi colon inside parentheses do not mean anything - line_flags |= LINE_FLAG_COMMENT_SEMICOLON; - } else if (c == '%') { - // discard this character - } else if (c == '\n') { // found the newline, so mark the end and return true - line[index] = '\0'; - return true; - } else { // add characters to the line - if (!line_flags) { - c = toupper(c); // make upper case - line[index] = c; - index++; - } - } - if (index == 255) { // name is too long so return false - line[index] = '\0'; - report_status_message(STATUS_OVERFLOW, SD_client); + if (len >= maxlen) { return false; } + char c = myFile.read(); + if (c == '\n') { + break; + } + line[len++] = c; } - // some files end without a newline - if (index != 0) { - line[index] = '\0'; - return true; - } else // empty line after new line - return false; + line[len] = '\0'; + return len || myFile.available(); } // return a percentage complete 50.5 = 50.5% diff --git a/Grbl_Esp32/grbl_sd.h b/Grbl_Esp32/grbl_sd.h index d8e841e1..20affa0d 100644 --- a/Grbl_Esp32/grbl_sd.h +++ b/Grbl_Esp32/grbl_sd.h @@ -44,7 +44,7 @@ uint8_t set_sd_state(uint8_t flag); void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client); boolean openFile(fs::FS& fs, const char* path); boolean closeFile(); -boolean readFileLine(char* line); +boolean readFileLine(char* line, int len); void readFile(fs::FS& fs, const char* path); float sd_report_perc_complete(); uint32_t sd_get_current_line_number(); diff --git a/Grbl_Esp32/grbl_trinamic.cpp b/Grbl_Esp32/grbl_trinamic.cppold similarity index 62% rename from Grbl_Esp32/grbl_trinamic.cpp rename to Grbl_Esp32/grbl_trinamic.cppold index f9b111f1..0df38be3 100644 --- a/Grbl_Esp32/grbl_trinamic.cpp +++ b/Grbl_Esp32/grbl_trinamic.cppold @@ -164,7 +164,7 @@ #ifdef C_TRINAMIC #ifdef C_DRIVER_TMC2130 - TMC2130Stepper TRINAMIC_c = TMC2130Stepper(C_CS_PIN, C_RSENSE, get_next_trinamic_driver_index()); + TMC2130Stepper TRINAMIC_C = TMC2130Stepper(C_CS_PIN, C_RSENSE, get_next_trinamic_driver_index()); #endif #ifdef C_DRIVER_TMC2209 TMC2209Stepper TRINAMIC_C = TMC2209Stepper(C_CS_PIN, C_RSENSE, get_next_trinamic_driver_index()); @@ -187,26 +187,102 @@ void Trinamic_Init() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "TMCStepper Init using Library Ver 0x%06x", TMCSTEPPER_VERSION); + + // On Slack Grbl_Esp32 channel, Mitch Bradley suggested. + // "Any pin that could possibly be implemented with a GPIO pin needs + // a pinMode before the first active use. pinMode(OUTPUT) does not have to + // precede digitalWrite(HIGH), as the following sequence is useful: + // digitalWrite(GPIOn, HIGH); + // pinMode(GPIOn, OUTPUT); + // The reason for that sequence is that, for pins that default to input mode/tri-state, + // setting it HIGH first prevents a low-going transition if the pin output register + // happens to be LOW when you select OUTPUT mode." + + // Notes on using I2S out: + // The TMC connected to the I2S out requires SPI clocking down (approximately 100 KHz) + // to work in concert with slow CS operations by I2S out. + +#ifdef X_DRIVER_TMC2130 + digitalWrite(X_CS_PIN, HIGH); + pinMode(X_CS_PIN, OUTPUT); + TRINAMIC_X.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef X2_DRIVER_TMC2130 + digitalWrite(X2_CS_PIN, HIGH); + pinMode(X2_CS_PIN, OUTPUT); + TRINAMIC_X2.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef Y_DRIVER_TMC2130 + digitalWrite(Y_CS_PIN, HIGH); + pinMode(Y_CS_PIN, OUTPUT); + TRINAMIC_Y.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef Y2_DRIVER_TMC2130 + digitalWrite(Y2_CS_PIN, HIGH); + pinMode(Y2_CS_PIN, OUTPUT); + TRINAMIC_Y2.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef Z_DRIVER_TMC2130 + digitalWrite(Z_CS_PIN, HIGH); + pinMode(Z_CS_PIN, OUTPUT); + TRINAMIC_Z.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef Z2_DRIVER_TMC2130 + digitalWrite(Z2_CS_PIN, HIGH); + pinMode(Z2_CS_PIN, OUTPUT); + TRINAMIC_Z2.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef A_DRIVER_TMC2130 + digitalWrite(A_CS_PIN, HIGH); + pinMode(A_CS_PIN, OUTPUT); + TRINAMIC_A.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef A2_DRIVER_TMC2130 + digitalWrite(A2_CS_PIN, HIGH); + pinMode(A2_CS_PIN, OUTPUT); + TRINAMIC_A2.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef B_DRIVER_TMC2130 + digitalWrite(B_CS_PIN, HIGH); + pinMode(B_CS_PIN, OUTPUT); + TRINAMIC_B.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef B2_DRIVER_TMC2130 + digitalWrite(B2_CS_PIN, HIGH); + pinMode(B2_CS_PIN, OUTPUT); + TRINAMIC_B2.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef C_DRIVER_TMC2130 + digitalWrite(C_CS_PIN, HIGH); + pinMode(C_CS_PIN, OUTPUT); + TRINAMIC_C.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif +#ifdef C2_DRIVER_TMC2130 + digitalWrite(C2_CS_PIN, HIGH); + pinMode(C2_CS_PIN, OUTPUT); + TRINAMIC_C2.setSPISpeed(TRINAMIC_SPI_FREQ); +#endif + SPI.begin(); #ifdef USE_MACHINE_TRINAMIC_INIT machine_trinamic_setup(); return; #endif -#ifdef X_TRINAMIC +#ifdef X_TRINAMIC TRINAMIC_X.begin(); // Initiate pins and registries - trinamic_test_response(TRINAMIC_X.test_connection(), "X"); + trinamic_test_response(TRINAMIC_X.test_connection(), "X"); TRINAMIC_X.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_X.microsteps(settings.microsteps[X_AXIS]); - TRINAMIC_X.rms_current(settings.current[X_AXIS] * 1000.0, settings.hold_current[X_AXIS] / 100.0); + TRINAMIC_X.microsteps(axis_settings[X_AXIS]->microsteps->get()); + TRINAMIC_X.rms_current(axis_settings[X_AXIS]->run_current->get() * 1000.0, axis_settings[X_AXIS]->hold_current->get() / 100.0); TRINAMIC_X.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_X.pwm_autoscale(1); #endif -#ifdef X2_TRINAMIC +#ifdef X2_TRINAMIC TRINAMIC_X2.begin(); // Initiate pins and registries - trinamic_test_response(TRINAMIC_X2.test_connection(), "X2"); + trinamic_test_response(TRINAMIC_X2.test_connection(), "X2"); TRINAMIC_X2.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_X2.microsteps(settings.microsteps[X_AXIS]); - TRINAMIC_X2.rms_current(settings.current[X_AXIS] * 1000.0, settings.hold_current[X_AXIS] / 100.0); + TRINAMIC_X2.microsteps(axis_settings[X_AXIS]->microsteps->get()); + TRINAMIC_X2.rms_current(axis_settings[X_AXIS]->run_current->get() * 1000.0, axis_settings[X_AXIS]->hold_current->get() / 100.0); TRINAMIC_X2.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_X2.pwm_autoscale(1); #endif @@ -215,8 +291,8 @@ void Trinamic_Init() { TRINAMIC_Y.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_Y.test_connection(), "Y"); TRINAMIC_Y.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_Y.microsteps(settings.microsteps[Y_AXIS]); - TRINAMIC_Y.rms_current(settings.current[Y_AXIS] * 1000.0, settings.hold_current[Y_AXIS] / 100.0); + TRINAMIC_Y.microsteps(axis_settings[Y_AXIS]->microsteps->get()); + TRINAMIC_Y.rms_current(axis_settings[Y_AXIS]->run_current->get() * 1000.0, axis_settings[Y_AXIS]->hold_current->get() / 100.0); TRINAMIC_Y.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_Y.pwm_autoscale(1); #endif @@ -224,8 +300,8 @@ void Trinamic_Init() { TRINAMIC_Y2.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_Y2.test_connection(), "Y2"); TRINAMIC_Y2.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_Y2.microsteps(settings.microsteps[Y_AXIS]); - TRINAMIC_Y2.rms_current(settings.current[Y_AXIS] * 1000.0, settings.hold_current[Y_AXIS] / 100.0); + TRINAMIC_Y2.microsteps(axis_settings[Y_AXIS]->microsteps->get()); + TRINAMIC_Y2.rms_current(axis_settings[Y_AXIS]->run_current->get() * 1000.0, axis_settings[Y_AXIS]->hold_current->get() / 100.0); TRINAMIC_Y2.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_Y2.pwm_autoscale(1); #endif @@ -234,8 +310,8 @@ void Trinamic_Init() { TRINAMIC_Z.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_Z.test_connection(), "Z"); TRINAMIC_Z.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_Z.microsteps(settings.microsteps[Z_AXIS]); - TRINAMIC_Z.rms_current(settings.current[Z_AXIS] * 1000.0, settings.hold_current[Z_AXIS] / 100.0); + TRINAMIC_Z.microsteps(axis_settings[Z_AXIS]->microsteps->get()); + TRINAMIC_Z.rms_current(axis_settings[Z_AXIS]->run_current->get() * 1000.0, axis_settings[Z_AXIS]->hold_current->get() / 100.0); TRINAMIC_Z.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_Z.pwm_autoscale(1); #endif @@ -243,8 +319,8 @@ void Trinamic_Init() { TRINAMIC_Z2.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_Z2.test_connection(), "Z2"); TRINAMIC_Z2.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_Z2.microsteps(settings.microsteps[Z_AXIS]); - TRINAMIC_Z2.rms_current(settings.current[Z_AXIS] * 1000.0, settings.hold_current[Z_AXIS] / 100.0); + TRINAMIC_Z2.microsteps(axis_settings[Z_AXIS]->microsteps->get()); + TRINAMIC_Z2.rms_current(axis_settings[Z_AXIS]->run_current->get() * 1000.0, axis_settings[Z_AXIS]->hold_current->get() / 100.0); TRINAMIC_Z2.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_Z2.pwm_autoscale(1); #endif @@ -253,8 +329,8 @@ void Trinamic_Init() { TRINAMIC_A.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_A.test_connection(), "A"); TRINAMIC_A.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_A.microsteps(settings.microsteps[A_AXIS]); - TRINAMIC_A.rms_current(settings.current[A_AXIS] * 1000.0, settings.hold_current[A_AXIS] / 100.0); + TRINAMIC_A.microsteps(axis_settings[A_AXIS]->microsteps->get()); + TRINAMIC_A.rms_current(axis_settings[A_AXIS]->run_current->get() * 1000.0, axis_settings[A_AXIS]->hold_current->get() / 100.0); TRINAMIC_A.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_A.pwm_autoscale(1); #endif @@ -262,8 +338,8 @@ void Trinamic_Init() { TRINAMIC_A2.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_A2.test_connection(), "A2"); TRINAMIC_A2.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_A2.microsteps(settings.microsteps[A_AXIS]); - TRINAMIC_A2.rms_current(settings.current[A_AXIS] * 1000.0, settings.hold_current[A_AXIS] / 100.0); + TRINAMIC_A2.microsteps(axis_settings[A_AXIS]->microsteps->get()); + TRINAMIC_A2.rms_current(axis_settings[A_AXIS]->run_current->get() * 1000.0, axis_settings[A_AXIS]->hold_current->get() / 100.0); TRINAMIC_A2.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_A2.pwm_autoscale(1); #endif @@ -272,8 +348,8 @@ void Trinamic_Init() { TRINAMIC_B.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_B.test_connection(), "B"); TRINAMIC_B.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_B.microsteps(settings.microsteps[B_AXIS]); - TRINAMIC_B.rms_current(settings.current[B_AXIS] * 1000.0, settings.hold_current[B_AXIS] / 100.0); + TRINAMIC_B.microsteps(axis_settings[B_AXIS]->microsteps->get()); + TRINAMIC_B.rms_current(axis_settings[B_AXIS]->run_current->get() * 1000.0, axis_settings[B_AXIS]->hold_current->get() / 100.0); TRINAMIC_B.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_B.pwm_autoscale(1); #endif @@ -281,8 +357,8 @@ void Trinamic_Init() { TRINAMIC_B2.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_B2.test_connection(), "B2"); TRINAMIC_B2.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_B2.microsteps(settings.microsteps[B_AXIS]); - TRINAMIC_B2.rms_current(settings.current[B_AXIS] * 1000.0, settings.hold_current[B_AXIS] / 100.0); + TRINAMIC_B2.microsteps(axis_settings[B_AXIS]->microsteps->get()); + TRINAMIC_B2.rms_current(axis_settings[B_AXIS]->run_current->get() * 1000.0, axis_settings[B_AXIS]->hold_current->get() / 100.0); TRINAMIC_B2.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_B2.pwm_autoscale(1); #endif @@ -291,8 +367,8 @@ void Trinamic_Init() { TRINAMIC_C.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_C.test_connection(), "C"); TRINAMIC_C.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_C.microsteps(settings.microsteps[C_AXIS]); - TRINAMIC_C.rms_current(settings.current[C_AXIS] * 1000.0, settings.hold_current[C_AXIS] / 100.0); + TRINAMIC_C.microsteps(axis_settings[C_AXIS]->microsteps->get()); + TRINAMIC_C.rms_current(axis_settings[C_AXIS]->run_current->get() * 1000.0, axis_settings[C_AXIS]->hold_current->get() / 100.0); TRINAMIC_C.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_C.pwm_autoscale(1); #endif @@ -300,8 +376,8 @@ void Trinamic_Init() { TRINAMIC_C2.begin(); // Initiate pins and registries trinamic_test_response(TRINAMIC_C2.test_connection(), "C2"); TRINAMIC_C2.toff(TRINAMIC_DEFAULT_TOFF); - TRINAMIC_C2.microsteps(settings.microsteps[C_AXIS]); - TRINAMIC_C2.rms_current(settings.current[C_AXIS] * 1000.0, settings.hold_current[C_AXIS] / 100.0); + TRINAMIC_C2.microsteps(axis_settings[C_AXIS]->microsteps->get()); + TRINAMIC_C2.rms_current(axis_settings[C_AXIS]->run_current->get() * 1000.0, axis_settings[C_AXIS]->hold_current->get() / 100.0); TRINAMIC_C2.en_pwm_mode(1); // Enable extremely quiet stepping TRINAMIC_C2.pwm_autoscale(1); #endif @@ -310,58 +386,58 @@ void Trinamic_Init() { // Call this function called whenever $$ settings that affect the drivers are changed void trinamic_change_settings() { #ifdef X_TRINAMIC - TRINAMIC_X.microsteps(settings.microsteps[X_AXIS]); - TRINAMIC_X.rms_current(settings.current[X_AXIS] * 1000.0, settings.hold_current[X_AXIS] / 100.0); + TRINAMIC_X.microsteps(axis_settings[X_AXIS]->microsteps->get()); + TRINAMIC_X.rms_current(axis_settings[X_AXIS]->run_current->get() * 1000.0, axis_settings[X_AXIS]->hold_current->get() / 100.0); #endif #ifdef X2_TRINAMIC - TRINAMIC_X2.microsteps(settings.microsteps[X_AXIS]); - TRINAMIC_X2.rms_current(settings.current[X_AXIS] * 1000.0, settings.hold_current[X_AXIS] / 100.0); + TRINAMIC_X2.microsteps(axis_settings[X_AXIS]->microsteps->get()); + TRINAMIC_X2.rms_current(axis_settings[X_AXIS]->run_current->get() * 1000.0, axis_settings[X_AXIS]->hold_current->get() / 100.0); #endif #ifdef Y_TRINAMIC - TRINAMIC_Y.microsteps(settings.microsteps[Y_AXIS]); - TRINAMIC_Y.rms_current(settings.current[Y_AXIS] * 1000.0, settings.hold_current[Y_AXIS] / 100.0); + TRINAMIC_Y.microsteps(axis_settings[Y_AXIS]->microsteps->get()); + TRINAMIC_Y.rms_current(axis_settings[Y_AXIS]->run_current->get() * 1000.0, axis_settings[Y_AXIS]->hold_current->get() / 100.0); #endif #ifdef Y2_TRINAMIC - TRINAMIC_Y2.microsteps(settings.microsteps[Y_AXIS]); - TRINAMIC_Y2.rms_current(settings.current[Y_AXIS] * 1000.0, settings.hold_current[Y_AXIS] / 100.0); + TRINAMIC_Y2.microsteps(axis_settings[Y_AXIS]->microsteps->get()); + TRINAMIC_Y2.rms_current(axis_settings[Y_AXIS]->run_current->get() * 1000.0, axis_settings[Y_AXIS]->hold_current->get() / 100.0); #endif #ifdef Z_TRINAMIC - TRINAMIC_Z.microsteps(settings.microsteps[Z_AXIS]); - TRINAMIC_Z.rms_current(settings.current[Z_AXIS] * 1000.0, settings.hold_current[Z_AXIS] / 100.0); + TRINAMIC_Z.microsteps(axis_settings[Z_AXIS]->microsteps->get()); + TRINAMIC_Z.rms_current(axis_settings[Z_AXIS]->run_current->get() * 1000.0, axis_settings[Z_AXIS]->hold_current->get() / 100.0); #endif #ifdef Z2_TRINAMIC - TRINAMIC_Z2.microsteps(settings.microsteps[Z_AXIS]); - TRINAMIC_Z2.rms_current(settings.current[Z_AXIS] * 1000.0, settings.hold_current[Z_AXIS] / 100.0); + TRINAMIC_Z2.microsteps(axis_settings[Z_AXIS]->microsteps->get()); + TRINAMIC_Z2.rms_current(axis_settings[Z_AXIS]->run_current->get() * 1000.0, axis_settings[Z_AXIS]->hold_current->get() / 100.0); #endif #ifdef A_TRINAMIC - TRINAMIC_A.microsteps(settings.microsteps[A_AXIS]); - TRINAMIC_A.rms_current(settings.current[A_AXIS] * 1000.0, settings.hold_current[A_AXIS] / 100.0); + TRINAMIC_A.microsteps(axis_settings[A_AXIS]->microsteps->get()); + TRINAMIC_A.rms_current(axis_settings[A_AXIS]->run_current->get() * 1000.0, axis_settings[A_AXIS]->hold_current->get() / 100.0); #endif #ifdef A2_TRINAMIC - TRINAMIC_A2.microsteps(settings.microsteps[A_AXIS]); - TRINAMIC_A2.rms_current(settings.current[A_AXIS] * 1000.0, settings.hold_current[A_AXIS] / 100.0); + TRINAMIC_A2.microsteps(axis_settings[A_AXIS]->microsteps->get()); + TRINAMIC_A2.rms_current(axis_settings[A_AXIS]->run_current->get() * 1000.0, axis_settings[A_AXIS]->hold_current->get() / 100.0); #endif #ifdef B_TRINAMIC - TRINAMIC_B.microsteps(settings.microsteps[B_AXIS]); - TTRINAMIC_B.rms_current(settings.current[B_AXIS] * 1000.0, settings.hold_current[B_AXIS] / 100.0); + TRINAMIC_B.microsteps(axis_settings[B_AXIS]->microsteps->get()); + TRINAMIC_B.rms_current(axis_settings[B_AXIS]->run_current->get() * 1000.0, axis_settings[B_AXIS]->hold_current->get() / 100.0); #endif #ifdef B2_TRINAMIC - TRINAMIC_B2.microsteps(settings.microsteps[B_AXIS]); - TTRINAMIC_B2.rms_current(settings.current[B_AXIS] * 1000.0, settings.hold_current[B_AXIS] / 100.0); + TRINAMIC_B2.microsteps(axis_settings[B_AXIS]->microsteps->get()); + TRINAMIC_B2.rms_current(axis_settings[B_AXIS]->run_current->get() * 1000.0, axis_settings[B_AXIS]->hold_current->get() / 100.0); #endif #ifdef C_TRINAMIC - TRINAMIC_C.microsteps(settings.microsteps[C_AXIS]); - TRINAMIC_C.rms_current(settings.current[C_AXIS] * 1000.0, settings.hold_current[C_AXIS] / 100.0); + TRINAMIC_C.microsteps(axis_settings[C_AXIS]->microsteps->get()); + TRINAMIC_C.rms_current(axis_settings[C_AXIS]->run_current->get() * 1000.0, axis_settings[C_AXIS]->hold_current->get() / 100.0); #endif #ifdef C2_TRINAMIC - TRINAMIC_C2.microsteps(settings.microsteps[C_AXIS]); - TRINAMIC_C2.rms_current(settings.current[C_AXIS] * 1000.0, settings.hold_current[C_AXIS] / 100.0); + TRINAMIC_C2.microsteps(axis_settings[C_AXIS]->microsteps->get()); + TRINAMIC_C2.rms_current(axis_settings[C_AXIS]->run_current->get() * 1000.0, axis_settings[C_AXIS]->hold_current->get() / 100.0); #endif } @@ -440,12 +516,21 @@ void trinamic_stepper_enable(bool enable) { // returns the next spi index. We cannot preassign to axes because ganged (X2 type axes) might // need to be inserted into the order of axes. uint8_t get_next_trinamic_driver_index() { +#ifdef TRINAMIC_DAISY_CHAIN static uint8_t index = 1; // they start at 1 - #ifndef TRINAMIC_DAISY_CHAIN - return -1; - #else - return index++; - #endif + return index++; +#else + return -1; +#endif } +#ifdef USE_I2S_OUT +// +// Override default function and insert a short delay +// +void TMC2130Stepper::switchCSpin(bool state) { + digitalWrite(_pinCS, state); + delay(I2S_OUT_DELAY_MS); +} +#endif #endif diff --git a/Grbl_Esp32/grbl_trinamic.h b/Grbl_Esp32/grbl_trinamic.hold similarity index 97% rename from Grbl_Esp32/grbl_trinamic.h rename to Grbl_Esp32/grbl_trinamic.hold index 0ca10c12..50949e38 100644 --- a/Grbl_Esp32/grbl_trinamic.h +++ b/Grbl_Esp32/grbl_trinamic.hold @@ -29,6 +29,8 @@ #define TMC2130_RSENSE_DEFAULT 0.11f #define TMC5160_RSENSE_DEFAULT 0.075f +#define TRINAMIC_SPI_FREQ 100000 + #ifdef USE_TRINAMIC #include // https://github.com/teemuatlut/TMCStepper void Trinamic_Init(); diff --git a/Grbl_Esp32/grbl_unipolar.cpp b/Grbl_Esp32/grbl_unipolar.cppold similarity index 95% rename from Grbl_Esp32/grbl_unipolar.cpp rename to Grbl_Esp32/grbl_unipolar.cppold index 72c78aaa..6e2f2fa0 100644 --- a/Grbl_Esp32/grbl_unipolar.cpp +++ b/Grbl_Esp32/grbl_unipolar.cppold @@ -61,13 +61,13 @@ void unipolar_init() { void unipolar_step(uint8_t step_mask, uint8_t dir_mask) { #ifdef X_UNIPOLAR - X_Unipolar.step(step_mask & (1 << X_AXIS), dir_mask & (1 << X_AXIS)); + X_Unipolar.step(step_mask & bit(X_AXIS), dir_mask & bit(X_AXIS)); #endif #ifdef Y_UNIPOLAR - Y_Unipolar.step(step_mask & (1 << Y_AXIS), dir_mask & (1 << Y_AXIS)); + Y_Unipolar.step(step_mask & bit(Y_AXIS), dir_mask & bit(Y_AXIS)); #endif #ifdef Z_UNIPOLAR - Z_Unipolar.step(step_mask & (1 << Z_AXIS), dir_mask & (1 << ZX_AXIS)); + Z_Unipolar.step(step_mask & bit(Z_AXIS), dir_mask & bit(ZX_AXIS)); #endif } @@ -211,4 +211,4 @@ void Unipolar::step(bool step, bool dir_forward) { digitalWrite(_pin_phase2, _phase[2]); digitalWrite(_pin_phase3, _phase[3]); } -#endif \ No newline at end of file +#endif diff --git a/Grbl_Esp32/grbl_unipolar.h b/Grbl_Esp32/grbl_unipolar.hold similarity index 100% rename from Grbl_Esp32/grbl_unipolar.h rename to Grbl_Esp32/grbl_unipolar.hold diff --git a/Grbl_Esp32/i2s_out.cpp b/Grbl_Esp32/i2s_out.cpp new file mode 100644 index 00000000..11d88d07 --- /dev/null +++ b/Grbl_Esp32/i2s_out.cpp @@ -0,0 +1,907 @@ +/* + i2s_out.cpp + Part of Grbl_ESP32 + + Basic GPIO expander using the ESP32 I2S peripheral (output) + + 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 . +*/ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#include "config.h" + +#ifdef USE_I2S_OUT + +#include +#include +#include +#include +#include + +#include + +#include "Pins.h" +#include "i2s_out.h" + +// +// Configrations for DMA connected I2S +// +// One DMA buffer transfer takes about 2 ms +// I2S_OUT_DMABUF_LEN / I2S_SAMPLE_SIZE x I2S_OUT_USEC_PER_PULSE +// = 2000 / 4 x 4 +// = 2000us = 2ms +// If I2S_OUT_DMABUF_COUNT is 5, it will take about 10 ms for all the DMA buffer transfers to finish. +// +// Increasing I2S_OUT_DMABUF_COUNT has the effect of preventing buffer underflow, +// but on the other hand, it leads to a delay with pulse and/or non-pulse-generated I/Os. +// The number of I2S_OUT_DMABUF_COUNT should be chosen carefully. +// +// Reference information: +// FreeRTOS task time slice = portTICK_PERIOD_MS = 1 ms (ESP32 FreeRTOS port) +// +#define I2S_SAMPLE_SIZE 4 /* 4 bytes, 32 bits per sample */ +#define DMA_SAMPLE_COUNT I2S_OUT_DMABUF_LEN / I2S_SAMPLE_SIZE /* number of samples per buffer */ +#define SAMPLE_SAFE_COUNT (20/I2S_OUT_USEC_PER_PULSE) /* prevent buffer overrun (GRBL's $0 should be less than or equal 20) */ + +#ifdef USE_I2S_OUT_STREAM +typedef struct { + uint32_t **buffers; + uint32_t *current; + uint32_t rw_pos; + lldesc_t **desc; + xQueueHandle queue; +} i2s_out_dma_t; + +static i2s_out_dma_t o_dma; +static intr_handle_t i2s_out_isr_handle; +#endif + +// output value +static atomic_uint_least32_t i2s_out_port_data = ATOMIC_VAR_INIT(0); + +// inner lock +static portMUX_TYPE i2s_out_spinlock = portMUX_INITIALIZER_UNLOCKED; +#define I2S_OUT_ENTER_CRITICAL() portENTER_CRITICAL(&i2s_out_spinlock) +#define I2S_OUT_EXIT_CRITICAL() portEXIT_CRITICAL(&i2s_out_spinlock) +#define I2S_OUT_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_spinlock) +#define I2S_OUT_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_spinlock) + +static int i2s_out_initialized = 0; + +#ifdef USE_I2S_OUT_STREAM +static volatile uint32_t i2s_out_pulse_period; +static uint32_t i2s_out_remain_time_until_next_pulse; // Time remaining until the next pulse (μsec) +static volatile i2s_out_pulse_func_t i2s_out_pulse_func; +#endif + +static uint8_t i2s_out_ws_pin = 255; +static uint8_t i2s_out_bck_pin = 255; +static uint8_t i2s_out_data_pin = 255; + +enum i2s_out_pulser_status_t { + PASSTHROUGH = 0, // Static I2S mode.The i2s_out_write() reflected with very little delay + STEPPING, // Streaming step data. + WAITING, // Waiting for the step DMA completion +}; +static volatile i2s_out_pulser_status_t i2s_out_pulser_status = PASSTHROUGH; + +// outer lock +static portMUX_TYPE i2s_out_pulser_spinlock = portMUX_INITIALIZER_UNLOCKED; +#define I2S_OUT_PULSER_ENTER_CRITICAL() portENTER_CRITICAL(&i2s_out_pulser_spinlock) +#define I2S_OUT_PULSER_EXIT_CRITICAL() portEXIT_CRITICAL(&i2s_out_pulser_spinlock) +#define I2S_OUT_PULSER_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_pulser_spinlock) +#define I2S_OUT_PULSER_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_pulser_spinlock) + +// +// Internal functions +// +static inline void gpio_matrix_out_check(uint8_t gpio, uint32_t signal_idx, bool out_inv, bool oen_inv) { + //if pin == 255, do not need to configure + if (gpio != 255) { + PIN_FUNC_SELECT(GPIO_PIN_MUX_REG[gpio], PIN_FUNC_GPIO); + gpio_set_direction((gpio_num_t)gpio, (gpio_mode_t)GPIO_MODE_DEF_OUTPUT); + gpio_matrix_out(gpio, signal_idx, out_inv, oen_inv); + } +} + +static inline void i2s_out_single_data() { +#if I2S_OUT_NUM_BITS == 16 + uint32_t port_data = atomic_load(&i2s_out_port_data); + port_data <<= 16; // Shift needed. This specification is not spelled out in the manual. + I2S0.conf_single_data = port_data; // Apply port data in real-time (static I2S) +#else + I2S0.conf_single_data = atomic_load(&i2s_out_port_data); // Apply port data in real-time (static I2S) +#endif +} + +static inline void i2s_out_reset_fifo_without_lock() { + I2S0.conf.rx_fifo_reset = 1; + I2S0.conf.rx_fifo_reset = 0; + I2S0.conf.tx_fifo_reset = 1; + I2S0.conf.tx_fifo_reset = 0; +} + +static void IRAM_ATTR i2s_out_reset_fifo() { + I2S_OUT_ENTER_CRITICAL(); + i2s_out_reset_fifo_without_lock(); + I2S_OUT_EXIT_CRITICAL(); +} + +#ifdef USE_I2S_OUT_STREAM +static int IRAM_ATTR i2s_clear_dma_buffer(lldesc_t *dma_desc, uint32_t port_data) { + + uint32_t *buf = (uint32_t*)dma_desc->buf; + for (int i = 0; i < DMA_SAMPLE_COUNT; i++) { + buf[i] = port_data; + } + // Restore the buffer length. + // The length may have been changed short when the data was filled in to prevent buffer overrun. + dma_desc->length = I2S_OUT_DMABUF_LEN; + return 0; +} + +static int IRAM_ATTR i2s_clear_o_dma_buffers(uint32_t port_data) { + for (int buf_idx = 0; buf_idx < I2S_OUT_DMABUF_COUNT; buf_idx++) { + // Initialize DMA descriptor + o_dma.desc[buf_idx]->owner = 1; + o_dma.desc[buf_idx]->eof = 1; // set to 1 will trigger the interrupt + o_dma.desc[buf_idx]->sosf = 0; + o_dma.desc[buf_idx]->length = I2S_OUT_DMABUF_LEN; + o_dma.desc[buf_idx]->size = I2S_OUT_DMABUF_LEN; + o_dma.desc[buf_idx]->buf = (uint8_t *) o_dma.buffers[buf_idx]; + o_dma.desc[buf_idx]->offset = 0; + o_dma.desc[buf_idx]->qe.stqe_next = (lldesc_t *)((buf_idx < (I2S_OUT_DMABUF_COUNT - 1)) ? (o_dma.desc[buf_idx + 1]) : o_dma.desc[0]); + i2s_clear_dma_buffer(o_dma.desc[buf_idx], port_data); + } + return 0; +} +#endif + +static int IRAM_ATTR i2s_out_gpio_attach(uint8_t ws, uint8_t bck, uint8_t data) { + // Route the i2s pins to the appropriate GPIO + gpio_matrix_out_check(data, I2S0O_DATA_OUT23_IDX, 0, 0); + gpio_matrix_out_check(bck, I2S0O_BCK_OUT_IDX, 0, 0); + gpio_matrix_out_check(ws, I2S0O_WS_OUT_IDX, 0, 0); + return 0; +} + +#define I2S_OUT_DETACH_PORT_IDX 0x100 + +static int IRAM_ATTR i2s_out_gpio_detach(uint8_t ws, uint8_t bck, uint8_t data) { + // Route the i2s pins to the appropriate GPIO + gpio_matrix_out_check(ws, I2S_OUT_DETACH_PORT_IDX, 0, 0); + gpio_matrix_out_check(bck, I2S_OUT_DETACH_PORT_IDX, 0, 0); + gpio_matrix_out_check(data, I2S_OUT_DETACH_PORT_IDX, 0, 0); + return 0; +} + +static int IRAM_ATTR i2s_out_gpio_shiftout(uint32_t port_data) { + __digitalWrite(i2s_out_ws_pin, LOW); + for (int i = 0; i buf; + o_dma.rw_pos = 0; + // It reuses the oldest (just transferred) buffer with the name "current" + // and fills the buffer for later DMA. + I2S_OUT_PULSER_ENTER_CRITICAL(); // Lock pulser status + if (i2s_out_pulser_status == STEPPING) { + // + // Fillout the buffer for pulse + // + // To avoid buffer overflow, all of the maximum pulse width (normaly about 10us) + // is adjusted to be in a single buffer. + // DMA_SAMPLE_SAFE_COUNT is referred to as the margin value. + // Therefore, if a buffer is close to full and it is time to generate a pulse, + // the generation of the buffer is interrupted (the buffer length is shortened slightly) + // and the pulse generation is postponed until the next buffer is filled. + // + o_dma.rw_pos = 0; + while (o_dma.rw_pos < (DMA_SAMPLE_COUNT - SAMPLE_SAFE_COUNT)) { + // no data to read (buffer empty) + if (i2s_out_remain_time_until_next_pulse < I2S_OUT_USEC_PER_PULSE) { + // pulser status may change in pulse phase func, so I need to check it every time. + if (i2s_out_pulser_status == STEPPING) { + // fillout future DMA buffer (tail of the DMA buffer chains) + if (i2s_out_pulse_func != NULL) { + I2S_OUT_PULSER_EXIT_CRITICAL(); // Temporarily unlocked status lock as it may be locked in pulse callback. + (*i2s_out_pulse_func)(); // should be pushed into buffer max DMA_SAMPLE_SAFE_COUNT + I2S_OUT_PULSER_ENTER_CRITICAL(); // Lock again. + i2s_out_remain_time_until_next_pulse = i2s_out_pulse_period; + if (i2s_out_pulser_status == WAITING) { + // i2s_out_set_passthrough() has called from the pulse function. + // It needs to go into pass-through mode. + // This DMA descriptor must be a tail of the chain. + dma_desc->qe.stqe_next = NULL; // Cut the DMA descriptor ring. This allow us to identify the tail of the buffer. + } else if (i2s_out_pulser_status == PASSTHROUGH) { + // i2s_out_reset() has called during the execution of the pulse function. + // I2S has already in static mode, and buffers has cleared to zero. + // To prevent the pulse function from being called back, + // we assume that the buffer is already full. + i2s_out_remain_time_until_next_pulse = 0; // There is no need to fill the current buffer. + o_dma.rw_pos = DMA_SAMPLE_COUNT; // The buffer is full. + break; + } + continue; + } + } + } + // no pulse data in push buffer (pulse off or idle or callback is not defined) + buf[o_dma.rw_pos++] = atomic_load(&i2s_out_port_data); + if (i2s_out_remain_time_until_next_pulse >= I2S_OUT_USEC_PER_PULSE) { + i2s_out_remain_time_until_next_pulse -= I2S_OUT_USEC_PER_PULSE; + } else { + i2s_out_remain_time_until_next_pulse = 0; + } + } + // set filled length to the DMA descriptor + dma_desc->length = o_dma.rw_pos * I2S_SAMPLE_SIZE; + } else if (i2s_out_pulser_status == WAITING) { + i2s_clear_dma_buffer(dma_desc, 0); // Essentially, no clearing is required. I'll make sure I know when I've written something. + o_dma.rw_pos = 0; // If someone calls i2s_out_push_sample, make sure there is no buffer overflow + dma_desc->qe.stqe_next = NULL; // Cut the DMA descriptor ring. This allow us to identify the tail of the buffer. + } else { + // Stepper paused (passthrough state, static I2S control mode) + // In the passthrough mode, there is no need to fill the buffer with port_data. + i2s_clear_dma_buffer(dma_desc, 0); // Essentially, no clearing is required. I'll make sure I know when I've written something. + o_dma.rw_pos = 0; // If someone calls i2s_out_push_sample, make sure there is no buffer overflow + } + I2S_OUT_PULSER_EXIT_CRITICAL(); // Unlock pulser status + + return 0; +} + +// +// I2S out DMA Interrupts handler +// +static void IRAM_ATTR i2s_out_intr_handler(void *arg) { + lldesc_t *finish_desc; + portBASE_TYPE high_priority_task_awoken = pdFALSE; + + if (I2S0.int_st.out_eof || I2S0.int_st.out_total_eof) { + if (I2S0.int_st.out_total_eof) { + // This is tail of the DMA descriptors + I2S_OUT_ENTER_CRITICAL_ISR(); + // Stop FIFO DMA + I2S0.out_link.stop = 1; + // Disconnect DMA from FIFO + I2S0.fifo_conf.dscr_en = 0; //Unset this bit to disable I2S DMA mode. (R/W) + // Stop TX module + I2S0.conf.tx_start = 0; + I2S_OUT_EXIT_CRITICAL_ISR(); + } + // Get the descriptor of the last item in the linkedlist + finish_desc = (lldesc_t*) I2S0.out_eof_des_addr; + + // If the queue is full it's because we have an underflow, + // more than buf_count isr without new data, remove the front buffer + if (xQueueIsQueueFullFromISR(o_dma.queue)) { + lldesc_t *front_desc; + // Remove a descriptor from the DMA complete event queue + xQueueReceiveFromISR(o_dma.queue, &front_desc, &high_priority_task_awoken); + I2S_OUT_PULSER_ENTER_CRITICAL_ISR(); + uint32_t port_data = 0; + if (i2s_out_pulser_status == STEPPING) { + port_data = atomic_load(&i2s_out_port_data); + } + I2S_OUT_PULSER_EXIT_CRITICAL_ISR(); + for (int i = 0; i < DMA_SAMPLE_COUNT; i++) { + front_desc->buf[i] = port_data; + } + front_desc->length = I2S_OUT_DMABUF_LEN; + } + + // Send a DMA complete event to the I2S bitstreamer task with finished buffer + xQueueSendFromISR(o_dma.queue, &finish_desc, &high_priority_task_awoken); + } + + if (high_priority_task_awoken == pdTRUE) portYIELD_FROM_ISR(); + + // clear interrupt + I2S0.int_clr.val = I2S0.int_st.val; //clear pending interrupt +} + +// +// I2S bitstream generator task +// +static void IRAM_ATTR i2sOutTask(void* parameter) { + lldesc_t *dma_desc; + while (1) { + // Wait a DMA complete event from I2S isr + // (Block until a DMA transfer has complete) + xQueueReceive(o_dma.queue, &dma_desc, portMAX_DELAY); + o_dma.current = (uint32_t*)(dma_desc->buf); + // It reuses the oldest (just transferred) buffer with the name "current" + // and fills the buffer for later DMA. + I2S_OUT_PULSER_ENTER_CRITICAL(); // Lock pulser status + if (i2s_out_pulser_status == STEPPING) { + // + // Fillout the buffer for pulse + // + // To avoid buffer overflow, all of the maximum pulse width (normaly about 10us) + // is adjusted to be in a single buffer. + // DMA_SAMPLE_SAFE_COUNT is referred to as the margin value. + // Therefore, if a buffer is close to full and it is time to generate a pulse, + // the generation of the buffer is interrupted (the buffer length is shortened slightly) + // and the pulse generation is postponed until the next buffer is filled. + // + i2s_fillout_dma_buffer(dma_desc); + dma_desc->length = o_dma.rw_pos * I2S_SAMPLE_SIZE; + } else if (i2s_out_pulser_status == WAITING) { + if (dma_desc->qe.stqe_next == NULL) { + // Tail of the DMA descriptor found + // I2S TX module has alrewdy stopped by ISR + i2s_out_stop(); + i2s_clear_o_dma_buffers(0); // 0 for static I2S control mode (right ch. data is always 0) + // You need to set the status before calling i2s_out_start() + // because the process in i2s_out_start() is different depending on the status. + i2s_out_pulser_status = PASSTHROUGH; + i2s_out_start(); + } else { + // Processing a buffer slightly ahead of the tail buffer. + // We don't need to fill up the buffer by port_data any more. + i2s_clear_dma_buffer(dma_desc, 0); // Essentially, no clearing is required. I'll make sure I know when I've written something. + o_dma.rw_pos = 0; // If someone calls i2s_out_push_sample, make sure there is no buffer overflow + dma_desc->qe.stqe_next = NULL; // Cut the DMA descriptor ring. This allow us to identify the tail of the buffer. + } + } else { + // Stepper paused (passthrough state, static I2S control mode) + // In the passthrough mode, there is no need to fill the buffer with port_data. + i2s_clear_dma_buffer(dma_desc, 0); // Essentially, no clearing is required. I'll make sure I know when I've written something. + o_dma.rw_pos = 0; // If someone calls i2s_out_push_sample, make sure there is no buffer overflow + } + I2S_OUT_PULSER_EXIT_CRITICAL(); // Unlock pulser status + } +} +#endif + +// +// External funtions +// +void IRAM_ATTR i2s_out_delay() { +#ifdef USE_I2S_OUT_STREAM + I2S_OUT_PULSER_ENTER_CRITICAL(); + if (i2s_out_pulser_status == PASSTHROUGH) { + // Depending on the timing, it may not be reflected immediately, + // so wait twice as long just in case. + ets_delay_us(I2S_OUT_USEC_PER_PULSE * 2); + } else { + // Just wait until the data now registered in the DMA descripter + // is reflected in the I2S TX module via FIFO. + delay(I2S_OUT_DELAY_MS); + } + I2S_OUT_PULSER_EXIT_CRITICAL(); +#else + ets_delay_us(I2S_OUT_USEC_PER_PULSE * 2); +#endif +} + +void IRAM_ATTR i2s_out_write(uint8_t pin, uint8_t val) { + uint32_t bit = bit(pin); + if (val) { + atomic_fetch_or(&i2s_out_port_data, bit); + } else { + atomic_fetch_and(&i2s_out_port_data, ~bit); + } +#ifdef USE_I2S_OUT_STREAM + // It needs a lock for access, but I've given up because I need speed. + // This is not a problem as long as there is no overlap between the status change and digitalWrite(). + if (i2s_out_pulser_status == PASSTHROUGH) { + i2s_out_single_data(); + } +#else + i2s_out_single_data(); +#endif +} + +uint8_t IRAM_ATTR i2s_out_state(uint8_t pin) { + uint32_t port_data = atomic_load(&i2s_out_port_data); + return (!!(port_data & bit(pin))); +} + +uint32_t IRAM_ATTR i2s_out_push_sample(uint32_t num) { +#ifdef USE_I2S_OUT_STREAM + if (num > SAMPLE_SAFE_COUNT) { + return 0; + } + // push at least one sample (even if num is zero) + uint32_t port_data = atomic_load(&i2s_out_port_data); + uint32_t n = 0; + do { + o_dma.current[o_dma.rw_pos++] = port_data; + n++; + } while(n < num); + return n; +#else + return 0; +#endif +} + +int IRAM_ATTR i2s_out_set_passthrough() { + I2S_OUT_PULSER_ENTER_CRITICAL(); +#ifdef USE_I2S_OUT_STREAM + if (i2s_out_pulser_status == STEPPING) { + i2s_out_pulser_status = WAITING; // Start stopping the pulser + } +#else + i2s_out_pulser_status = PASSTHROUGH; +#endif + I2S_OUT_PULSER_EXIT_CRITICAL(); + return 0; +} + +int IRAM_ATTR i2s_out_set_stepping() { + I2S_OUT_PULSER_ENTER_CRITICAL(); +#ifdef USE_I2S_OUT_STREAM + if (i2s_out_pulser_status == STEPPING) { + // Re-entered (fail safe) + I2S_OUT_PULSER_EXIT_CRITICAL(); + return 0; + } + + if (i2s_out_pulser_status == WAITING) { + // Wait for complete DMAs + for(;;) { + I2S_OUT_PULSER_EXIT_CRITICAL(); + delay(I2S_OUT_DELAY_DMABUF_MS); + I2S_OUT_PULSER_ENTER_CRITICAL(); + if (i2s_out_pulser_status == WAITING) { + continue; + } + if (i2s_out_pulser_status == PASSTHROUGH) { + // DMA completed + break; + } + // Another function change the I2S state to STEPPING + I2S_OUT_PULSER_EXIT_CRITICAL(); + return 0; + } + } + + // Change I2S state from PASSTHROUGH to STEPPING + i2s_out_stop(); + uint32_t port_data = atomic_load(&i2s_out_port_data); + i2s_clear_o_dma_buffers(port_data); + + // You need to set the status before calling i2s_out_start() + // because the process in i2s_out_start() is different depending on the status. + i2s_out_pulser_status = STEPPING; + i2s_out_start(); +#else + i2s_out_pulser_status = STEPPING; +#endif + I2S_OUT_PULSER_EXIT_CRITICAL(); + return 0; +} + +int IRAM_ATTR i2s_out_set_pulse_period(uint32_t period) { +#ifdef USE_I2S_OUT_STREAM + i2s_out_pulse_period = period; +#endif + return 0; +} + +int IRAM_ATTR i2s_out_set_pulse_callback(i2s_out_pulse_func_t func) { +#ifdef USE_I2S_OUT_STREAM + i2s_out_pulse_func = func; +#endif + return 0; +} + +int IRAM_ATTR i2s_out_reset() { + I2S_OUT_PULSER_ENTER_CRITICAL(); + i2s_out_stop(); +#ifdef USE_I2S_OUT_STREAM + if (i2s_out_pulser_status == STEPPING) { + uint32_t port_data = atomic_load(&i2s_out_port_data); + i2s_clear_o_dma_buffers(port_data); + } else if (i2s_out_pulser_status == WAITING) { + i2s_clear_o_dma_buffers(0); + i2s_out_pulser_status = PASSTHROUGH; + } +#endif + // You need to set the status before calling i2s_out_start() + // because the process in i2s_out_start() is different depending on the status. + i2s_out_start(); + I2S_OUT_PULSER_EXIT_CRITICAL(); + return 0; +} + +// +// Initialize funtion (external function) +// +int IRAM_ATTR i2s_out_init(i2s_out_init_t &init_param) { + if (i2s_out_initialized) { + // already initialized + return -1; + } + + atomic_store(&i2s_out_port_data, init_param.init_val); + + // To make sure hardware is enabled before any hardware register operations. + periph_module_reset(PERIPH_I2S0_MODULE); + periph_module_enable(PERIPH_I2S0_MODULE); + + // Route the i2s pins to the appropriate GPIO + i2s_out_gpio_attach(init_param.ws_pin, init_param.bck_pin, init_param.data_pin); + + /** + * Each i2s transfer will take + * fpll = PLL_D2_CLK -- clka_en = 0 + * + * fi2s = fpll / N + b/a -- N + b/a = clkm_div_num + * fi2s = 160MHz / 2 + * fi2s = 80MHz + * + * fbclk = fi2s / M -- M = tx_bck_div_num + * fbclk = 80MHz / 2 + * fbclk = 40MHz + * + * fwclk = fbclk / 32 + * + * for fwclk = 250kHz(16-bit: 4µS pulse time), 125kHz(32-bit: 8μS pulse time) + * N = 10, b/a = 0 + * M = 2 + * for fwclk = 500kHz(16-bit: 2µS pulse time), 250kHz(32-bit: 4μS pulse time) + * N = 5, b/a = 0 + * M = 2 + * for fwclk = 1000kHz(16-bit: 1µS pulse time), 500kHz(32-bit: 2μS pulse time) + * N = 2, b/a = 2/1 (N + b/a = 2.5) + * M = 2 + */ + +#ifdef USE_I2S_OUT_STREAM + // Allocate the array of pointers to the buffers + o_dma.buffers = (uint32_t **)malloc(sizeof(uint32_t*) * I2S_OUT_DMABUF_COUNT); + if (o_dma.buffers == nullptr) return -1; + + // Allocate each buffer that can be used by the DMA controller + for (int buf_idx = 0; buf_idx < I2S_OUT_DMABUF_COUNT; buf_idx++) { + o_dma.buffers[buf_idx] = (uint32_t*) heap_caps_calloc(1, I2S_OUT_DMABUF_LEN, MALLOC_CAP_DMA); + if (o_dma.buffers[buf_idx] == nullptr) return -1; + } + + // Allocate the array of DMA descriptors + o_dma.desc = (lldesc_t**) malloc(sizeof(lldesc_t*) * I2S_OUT_DMABUF_COUNT); + if (o_dma.desc == nullptr) return -1; + + // Allocate each DMA descriptor that will be used by the DMA controller + for (int buf_idx = 0; buf_idx < I2S_OUT_DMABUF_COUNT; buf_idx++) { + o_dma.desc[buf_idx] = (lldesc_t*) heap_caps_malloc(sizeof(lldesc_t), MALLOC_CAP_DMA); + if (o_dma.desc[buf_idx] == nullptr) return -1; + } + + // Initialize + i2s_clear_o_dma_buffers(init_param.init_val); + o_dma.rw_pos = 0; + o_dma.current = NULL; + o_dma.queue = xQueueCreate(I2S_OUT_DMABUF_COUNT, sizeof(uint32_t *)); + + // Set the first DMA descriptor + I2S0.out_link.addr = (uint32_t)o_dma.desc[0]; +#endif + + // stop i2s + I2S0.out_link.stop = 1; + I2S0.conf.tx_start = 0; + + I2S0.int_clr.val = I2S0.int_st.val; //clear pending interrupt + + // + // i2s_param_config + // + + // configure I2S data port interface. + i2s_out_reset_fifo(); + + //reset i2s + I2S0.conf.tx_reset = 1; + I2S0.conf.tx_reset = 0; + I2S0.conf.rx_reset = 1; + I2S0.conf.rx_reset = 0; + + //reset dma + I2S0.lc_conf.in_rst = 1; // Set this bit to reset in DMA FSM. (R/W) + I2S0.lc_conf.in_rst = 0; + I2S0.lc_conf.out_rst = 1; // Set this bit to reset out DMA FSM. (R/W) + I2S0.lc_conf.out_rst = 0; + + //Enable and configure DMA + I2S0.lc_conf.check_owner = 0; + I2S0.lc_conf.out_loop_test = 0; + I2S0.lc_conf.out_auto_wrback = 0; // Disable auto outlink-writeback when all the data has been transmitted + I2S0.lc_conf.out_data_burst_en = 0; + I2S0.lc_conf.outdscr_burst_en = 0; + I2S0.lc_conf.out_no_restart_clr = 0; + I2S0.lc_conf.indscr_burst_en = 0; +#ifdef USE_I2S_OUT_STREAM + I2S0.lc_conf.out_eof_mode = 1; // I2S_OUT_EOF_INT generated when DMA has popped all data from the FIFO; +#endif + I2S0.conf2.lcd_en = 0; + I2S0.conf2.camera_en = 0; + I2S0.pdm_conf.pcm2pdm_conv_en = 0; + I2S0.pdm_conf.pdm2pcm_conv_en = 0; + + I2S0.fifo_conf.dscr_en = 0; + +#ifdef USE_I2S_OUT_STREAM + if (i2s_out_pulser_status == STEPPING) { + // Stream output mode + I2S0.conf_chan.tx_chan_mod = 4; // 3:right+constant 4:left+constant (when tx_msb_right = 1) + I2S0.conf_single_data = 0; + } else { + // Static output mode + I2S0.conf_chan.tx_chan_mod = 3; // 3:right+constant 4:left+constant (when tx_msb_right = 1) + I2S0.conf_single_data = init_param.init_val; + } +#else + // For the static output mode + I2S0.conf_chan.tx_chan_mod = 3; // 3:right+constant 4:left+constant (when tx_msb_right = 1) + I2S0.conf_single_data = init_param.init_val; // initial constant value +#endif +#if I2S_OUT_NUM_BITS == 16 + I2S0.fifo_conf.tx_fifo_mod = 0; // 0: 16-bit dual channel data, 3: 32-bit single channel data + I2S0.fifo_conf.rx_fifo_mod = 0; // 0: 16-bit dual channel data, 3: 32-bit single channel data + I2S0.sample_rate_conf.tx_bits_mod = 16; // default is 16-bits + I2S0.sample_rate_conf.rx_bits_mod = 16; // default is 16-bits +#else + I2S0.fifo_conf.tx_fifo_mod = 3; // 0: 16-bit dual channel data, 3: 32-bit single channel data + I2S0.fifo_conf.rx_fifo_mod = 3; // 0: 16-bit dual channel data, 3: 32-bit single channel data + // Data width is 32-bit. Forgetting this setting will result in a 16-bit transfer. + I2S0.sample_rate_conf.tx_bits_mod = 32; + I2S0.sample_rate_conf.rx_bits_mod = 32; +#endif + I2S0.conf.tx_mono = 0; // Set this bit to enable transmitter’s mono mode in PCM standard mode. + + I2S0.conf_chan.rx_chan_mod = 1; // 1: right+right + I2S0.conf.rx_mono = 0; + +#ifdef USE_I2S_OUT_STREAM + I2S0.fifo_conf.dscr_en = 1; //connect DMA to fifo +#endif + I2S0.conf.tx_start = 0; + I2S0.conf.rx_start = 0; + + I2S0.conf.tx_msb_right = 1; // Set this bit to place right-channel data at the MSB in the transmit FIFO. + I2S0.conf.tx_right_first = 0; // Setting this bit allows the right-channel data to be sent first. + + I2S0.conf.tx_slave_mod = 0; // Master +#ifdef USE_I2S_OUT_STREAM + I2S0.fifo_conf.tx_fifo_mod_force_en = 1; //The bit should always be set to 1. +#endif + I2S0.pdm_conf.rx_pdm_en = 0; // Set this bit to enable receiver’s PDM mode. + I2S0.pdm_conf.tx_pdm_en = 0; // Set this bit to enable transmitter’s PDM mode. + + // I2S_COMM_FORMAT_I2S_LSB + I2S0.conf.tx_short_sync = 0; // Set this bit to enable transmitter in PCM standard mode. + I2S0.conf.rx_short_sync = 0; // Set this bit to enable receiver in PCM standard mode. + I2S0.conf.tx_msb_shift = 0; // Do not use the Philips standard to avoid bit-shifting + I2S0.conf.rx_msb_shift = 0; // Do not use the Philips standard to avoid bit-shifting + + // + // i2s_set_clk + // + + // set clock (fi2s) 160MHz / 5 + I2S0.clkm_conf.clka_en = 0; // Use 160 MHz PLL_D2_CLK as reference + // N + b/a = 0 +#if I2S_OUT_NUM_BITS == 16 + // N = 10 + I2S0.clkm_conf.clkm_div_num = 10; // minimum value of 2, reset value of 4, max 256 (I²S clock divider’s integral value) +#else + // N = 5 + I2S0.clkm_conf.clkm_div_num = 5; // minimum value of 2, reset value of 4, max 256 (I²S clock divider’s integral value) +#endif + // b/a = 0 + I2S0.clkm_conf.clkm_div_b = 0; // 0 at reset + I2S0.clkm_conf.clkm_div_a = 0; // 0 at reset, what about divide by 0? (not an issue) + + // Bit clock configuration bit in transmitter mode. + // fbck = fi2s / tx_bck_div_num = (160 MHz / 5) / 2 = 16 MHz + I2S0.sample_rate_conf.tx_bck_div_num = 2; // minimum value of 2 defaults to 6 + I2S0.sample_rate_conf.rx_bck_div_num = 2; + +#ifdef USE_I2S_OUT_STREAM + // Enable TX interrupts (DMA Interrupts) + I2S0.int_ena.out_eof = 1; // Triggered when rxlink has finished sending a packet. + I2S0.int_ena.out_dscr_err = 0; // Triggered when invalid rxlink descriptors are encountered. + I2S0.int_ena.out_total_eof = 1; // Triggered when all transmitting linked lists are used up. + I2S0.int_ena.out_done = 0; // Triggered when all transmitted and buffered data have been read. + + // default pulse callback period (μsec) + i2s_out_pulse_period = init_param.pulse_period; + i2s_out_pulse_func = init_param.pulse_func; + + // Create the task that will feed the buffer + xTaskCreatePinnedToCore(i2sOutTask, + "I2SOutTask", + 1024 * 10, + NULL, + 1, + nullptr, + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + ); + + // Allocate and Enable the I2S interrupt + esp_intr_alloc(ETS_I2S0_INTR_SOURCE, 0, i2s_out_intr_handler, nullptr, &i2s_out_isr_handle); + esp_intr_enable(i2s_out_isr_handle); +#endif + + // Remember GPIO pin numbers + i2s_out_ws_pin = init_param.ws_pin; + i2s_out_bck_pin = init_param.bck_pin; + i2s_out_data_pin = init_param.data_pin; + i2s_out_initialized = 1; + + // Start the I2S peripheral + i2s_out_start(); + + return 0; +} + +#ifndef I2S_OUT_WS +#define I2S_OUT_WS GPIO_NUM_17 +#endif +#ifndef I2S_OUT_BCK +#define I2S_OUT_BCK GPIO_NUM_22 +#endif +#ifndef I2S_OUT_DATA +#define I2S_OUT_DATA GPIO_NUM_21 +#endif +#ifndef I2S_OUT_INIT_VAL +#define I2S_OUT_INIT_VAL 0 +#endif +/* + Initialize I2S out by default parameters. + + return -1 ... already initialized +*/ +int IRAM_ATTR i2s_out_init() { + i2s_out_init_t default_param = { + .ws_pin = I2S_OUT_WS, + .bck_pin = I2S_OUT_BCK, + .data_pin = I2S_OUT_DATA, + .pulse_func = NULL, + .pulse_period = I2S_OUT_USEC_PER_PULSE, + .init_val = I2S_OUT_INIT_VAL, + }; + return i2s_out_init(default_param); +} + +#endif diff --git a/Grbl_Esp32/i2s_out.h b/Grbl_Esp32/i2s_out.h new file mode 100644 index 00000000..4e88ac89 --- /dev/null +++ b/Grbl_Esp32/i2s_out.h @@ -0,0 +1,191 @@ +/* + i2s_out.h + Part of Grbl_ESP32 + Header for basic GPIO expander using the ESP32 I2S peripheral + 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 . +*/ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#ifndef i2s_out_h +#define i2s_out_h + +// It should be included at the outset to know the machine configuration. +#include "config.h" + +// If USE_I2S_OUT_STREAM is defined +// but the prerequisite USE_I2S_OUT is not defined, +// it is forced to be defined. +#ifdef USE_I2S_OUT_STREAM + #ifndef USE_I2S_OUT + #define USE_I2S_OUT + #endif +#endif + +#ifdef USE_I2S_OUT +#include + +/* Assert */ +#if defined(I2S_OUT_NUM_BITS) + #if (I2S_OUT_NUM_BITS != 16) && (I2S_OUT_NUM_BITS != 32) + #error "I2S_OUT_NUM_BITS should be 16 or 32" + #endif +#else + #define I2S_OUT_NUM_BITS 32 +#endif + +#define I2SO(n) (I2S_OUT_PIN_BASE + n) + +/* 16-bit mode: 1000000 usec / ((160000000 Hz) / 10 / 2) x 16 bit/pulse x 2(stereo) = 4 usec/pulse */ +/* 32-bit mode: 1000000 usec / ((160000000 Hz) / 5 / 2) x 32 bit/pulse x 2(stereo) = 4 usec/pulse */ +#define I2S_OUT_USEC_PER_PULSE 4 + +#define I2S_OUT_DMABUF_COUNT 5 /* number of DMA buffers to store data */ +#define I2S_OUT_DMABUF_LEN 2000 /* maximum size in bytes (4092 is DMA's limit) */ + +#define I2S_OUT_DELAY_DMABUF_MS (I2S_OUT_DMABUF_LEN / sizeof(uint32_t) * I2S_OUT_USEC_PER_PULSE / 1000) +#define I2S_OUT_DELAY_MS (I2S_OUT_DELAY_DMABUF_MS * (I2S_OUT_DMABUF_COUNT + 1)) + +typedef void (*i2s_out_pulse_func_t)(void); + +typedef struct { + /* + I2S bitstream (32-bits): Transfers from MSB(bit31) to LSB(bit0) in sequence + ------------------time line------------------------> + Left Channel Right Channel + ws ________________________________~~~~... + bck _~_~_~_~_~_~_~_~_~_~_~_~_~_~_~_~_~_~... + data vutsrqponmlkjihgfedcba9876543210 + XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX + ^ + Latches the X bits when ws is switched to High + If I2S_OUT_PIN_BASE is set to 128, + bit0:Expanded GPIO 128, 1: Expanded GPIO 129, ..., v: Expanded GPIO 159 + */ + uint8_t ws_pin; + uint8_t bck_pin; + uint8_t data_pin; + i2s_out_pulse_func_t pulse_func; + uint32_t pulse_period; // aka step rate. + uint32_t init_val; +} i2s_out_init_t; + +/* + Initialize I2S out by parameters. + return -1 ... already initialized +*/ +int i2s_out_init(i2s_out_init_t &init_param); + +/* + Initialize I2S out by default parameters. + i2s_out_init_t default_param = { + .ws_pin = I2S_OUT_WS, + .bck_pin = I2S_OUT_BCK, + .data_pin = I2S_OUT_DATA, + .pulse_func = NULL, + .pulse_period = I2S_OUT_USEC_PER_PULSE, + .init_val = I2S_OUT_INIT_VAL, + }; + return -1 ... already initialized +*/ +int i2s_out_init(); + +/* + Get a bit state from the internal pin state var. + pin: expanded pin No. (0..31) +*/ +uint8_t i2s_out_state(uint8_t pin); + +/* + Set a bit in the internal pin state var. (not written electrically) + pin: expanded pin No. (0..31) + val: bit value(0 or not 0) +*/ +void i2s_out_write(uint8_t pin, uint8_t val); + +/* + Set current pin state to the I2S bitstream buffer + (This call will generate a future I2S_OUT_USEC_PER_PULSE μs x N bitstream) + num: Number of samples to be generated + The number of samples is limited to (20 / I2S_OUT_USEC_PER_PULSE). + return: number of puhsed samples + 0 .. no space for push + */ +uint32_t i2s_out_push_sample(uint32_t num); + +/* + Set pulser mode to passtrough + After this function is called, + the callback function to generate the pulse data + will not be called. + */ +int i2s_out_set_passthrough(); + +/* + Set pulser mode to stepping + After this function is called, + the callback function to generate stepping pulse data + will be called. + */ +int i2s_out_set_stepping(); + +/* + Dynamically delay until the Shift Register Pin changes + according to the current I2S processing state and mode. + */ +void i2s_out_delay(); + +/* + Set the pulse callback period in microseconds + (like the timer period for the ISR) + */ +int i2s_out_set_pulse_period(uint32_t period); + +/* + Register a callback function to generate pulse data + */ +int i2s_out_set_pulse_callback(i2s_out_pulse_func_t func); + +/* + Reset i2s I/O expander + - Stop ISR/DMA + - Clear DMA buffer with the current expanded GPIO bits + - Retart ISR/DMA + */ +int i2s_out_reset(); + +#endif + +/* + Reference: "ESP32 Technical Reference Manual" by Espressif Systems + https://www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf + */ +#endif diff --git a/Grbl_Esp32/jog.cpp b/Grbl_Esp32/jog.cpp index 9bd92df9..902b48f8 100644 --- a/Grbl_Esp32/jog.cpp +++ b/Grbl_Esp32/jog.cpp @@ -33,7 +33,7 @@ uint8_t jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { #ifdef USE_LINE_NUMBERS pl_data->line_number = gc_block->values.n; #endif - if (bit_istrue(settings.flags, BITFLAG_SOFT_LIMIT_ENABLE)) { + if (soft_limits->get()) { if (system_check_travel_limits(gc_block->values.xyz)) return (STATUS_TRAVEL_EXCEEDED); } // Valid jog command. Plan, set state, and execute. diff --git a/Grbl_Esp32/machine.h b/Grbl_Esp32/machine.h index 37a1ce6c..3f987a94 100644 --- a/Grbl_Esp32/machine.h +++ b/Grbl_Esp32/machine.h @@ -16,6 +16,7 @@ PWM // !!! For initial testing, start with test_drive.h which disables // all I/O pins +// #include "Machines/atari_1020.h" #include "Machines/test_drive.h" // !!! For actual use, change the line above to select a board diff --git a/Grbl_Esp32/machine_common.h b/Grbl_Esp32/machine_common.h index 6eface1f..ea3e20fb 100644 --- a/Grbl_Esp32/machine_common.h +++ b/Grbl_Esp32/machine_common.h @@ -1,7 +1,7 @@ #ifndef _machine_common_h #define _machine_common_h -#ifndef SPINDLE_TYPE +#ifndef SPINDLE_TYPE #define SPINDLE_TYPE SPINDLE_TYPE_PWM #endif @@ -31,30 +31,8 @@ // =============== Don't change or comment these out ====================== // They are for legacy purposes and will not affect your I/O -#define X_STEP_BIT 0 -#define Y_STEP_BIT 1 -#define Z_STEP_BIT 2 -#define A_STEP_BIT 3 -#define B_STEP_BIT 4 -#define C_STEP_BIT 5 #define STEP_MASK B111111 -#define X_DIRECTION_BIT 0 -#define Y_DIRECTION_BIT 1 -#define Z_DIRECTION_BIT 2 -#define A_DIRECTION_BIT 3 -#define B_DIRECTION_BIT 4 -#define C_DIRECTION_BIT 5 - -#define X_LIMIT_BIT 0 -#define Y_LIMIT_BIT 1 -#define Z_LIMIT_BIT 2 -#define A_LIMIT_BIT 3 -#define B_LIMIT_BIT 4 -#define C_LIMIT_BIT 5 - #define PROBE_MASK 1 -#define CONTROL_MASK B1111 - #endif // _machine_common_h diff --git a/Grbl_Esp32/motion_control.cpp b/Grbl_Esp32/motion_control.cpp index 2c34bf94..304c545d 100644 --- a/Grbl_Esp32/motion_control.cpp +++ b/Grbl_Esp32/motion_control.cpp @@ -24,6 +24,13 @@ #include "grbl.h" +// M_PI is not defined in standard C/C++ but some compilers +// support it anyway. The following suppresses Intellisense +// problem reports. +#ifndef M_PI + #define M_PI 3.14159265358979323846 +#endif + uint8_t ganged_mode = SQUARING_MODE_DUAL; @@ -46,7 +53,7 @@ void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position) { void mc_line(float* target, plan_line_data_t* pl_data) { // If enabled, check for soft limit violations. Placed here all line motions are picked up // from everywhere in Grbl. - if (bit_istrue(settings.flags, BITFLAG_SOFT_LIMIT_ENABLE)) { + if (soft_limits->get()) { // NOTE: Block jog state. Jogging is a special case and soft limits are handled independently. if (sys.state != STATE_JOG) limits_soft_check(target); } @@ -84,7 +91,7 @@ void mc_line(float* target, plan_line_data_t* pl_data) { // the direction of helical travel, radius == circle radius, isclockwise boolean. Used // for vector transformation direction. // The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance -// of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal +// of each segment is configured in the arc_tolerance setting, which is defined to be the maximum normal // distance from segment to the circle when the end points both lie on the circle. void mc_arc(float* target, plan_line_data_t* pl_data, float* position, float* offset, float radius, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc) { @@ -108,11 +115,11 @@ void mc_arc(float* target, plan_line_data_t* pl_data, float* position, float* of if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) angular_travel += 2 * M_PI; } // NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to - // (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit + // (2x) arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit // is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation. // For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases. uint16_t segments = floor(fabs(0.5 * angular_travel * radius) / - sqrt(settings.arc_tolerance * (2 * radius - settings.arc_tolerance))); + sqrt(arc_tolerance->get() * (2 * radius - arc_tolerance->get()))); if (segments) { // Multiply inverse feed_rate to compensate for the fact that this movement is approximated // by a number of discrete segments. The inverse feed_rate should be correct for the sum of @@ -312,12 +319,14 @@ void mc_homing_cycle(uint8_t cycle_mask) { #endif } protocol_execute_realtime(); // Check for reset and set system abort. - if (sys.abort) return; // Did not complete. Alarm state set by mc_alarm. + if (sys.abort) { + return; // Did not complete. Alarm state set by mc_alarm. + } // Homing cycle complete! Setup system for normal operation. // ------------------------------------------------------------------------------------- // Sync gcode parser and planner positions to homed position. gc_sync_position(); - plan_sync_position(); + plan_sync_position(); #ifdef USE_KINEMATICS // This give kinematics a chance to do something after normal homing kinematics_post_homing(); @@ -447,8 +456,10 @@ void mc_reset() { } else system_set_exec_alarm(EXEC_ALARM_ABORT_CYCLE); st_go_idle(); // Force kill steppers. Position has likely been lost. } -#ifdef USE_GANGED_AXES ganged_mode = SQUARING_MODE_DUAL; // in case an error occurred during squaring + +#ifdef USE_I2S_OUT_STREAM + i2s_out_reset(); #endif } } diff --git a/Grbl_Esp32/notifications_service.cpp b/Grbl_Esp32/notifications_service.cpp index 16ce87d1..1a4ee36c 100644 --- a/Grbl_Esp32/notifications_service.cpp +++ b/Grbl_Esp32/notifications_service.cpp @@ -28,15 +28,11 @@ // - https://github.com/CosmicBoris/ESP8266SMTP // - https://www.electronicshub.org/send-an-email-using-esp8266/ -#include "config.h" -#ifdef ENABLE_NOTIFICATIONS #include "grbl.h" -#include "commands.h" +#ifdef ENABLE_NOTIFICATIONS #include "notifications_service.h" #include -#include #include -#include "wificonfig.h" #define PUSHOVERTIMEOUT 5000 #define PUSHOVERSERVER "api.pushover.net" @@ -273,28 +269,17 @@ bool NotificationsService::sendLineMSG(const char* title, const char* message) { } //Email#serveraddress:port bool NotificationsService::getPortFromSettings() { - Preferences prefs; - String defV = DEFAULT_TOKEN; - prefs.begin(NAMESPACE, true); - String tmp = prefs.getString(NOTIFICATION_TS, defV); - prefs.end(); + String tmp = notification_ts->get(); int pos = tmp.lastIndexOf(':'); if (pos == -1) return false; _port = tmp.substring(pos + 1).toInt(); log_d("port : %d", _port); - if (_port > 0) - return true; - else - return false; + return _port > 0; } //Email#serveraddress:port bool NotificationsService::getServerAddressFromSettings() { - Preferences prefs; - String defV = DEFAULT_TOKEN; - prefs.begin(NAMESPACE, true); - String tmp = prefs.getString(NOTIFICATION_TS, defV); - prefs.end(); + String tmp = notification_ts->get(); int pos1 = tmp.indexOf('#'); int pos2 = tmp.lastIndexOf(':'); if ((pos1 == -1) || (pos2 == -1)) @@ -306,11 +291,7 @@ bool NotificationsService::getServerAddressFromSettings() { } //Email#serveraddress:port bool NotificationsService::getEmailFromSettings() { - Preferences prefs; - String defV = DEFAULT_TOKEN; - prefs.begin(NAMESPACE, true); - String tmp = prefs.getString(NOTIFICATION_TS, defV); - prefs.end(); + String tmp = notification_ts->get(); int pos = tmp.indexOf('#'); if (pos == -1) return false; @@ -322,42 +303,34 @@ bool NotificationsService::getEmailFromSettings() { bool NotificationsService::begin() { - bool res = true; end(); - Preferences prefs; - String defV = DEFAULT_TOKEN; - prefs.begin(NAMESPACE, true); - _notificationType = prefs.getChar(NOTIFICATION_TYPE, DEFAULT_NOTIFICATION_TYPE); + _notificationType = notification_type->get(); switch (_notificationType) { case 0: //no notification = no error but no start return true; case ESP_PUSHOVER_NOTIFICATION: - _token1 = prefs.getString(NOTIFICATION_T1, defV); - _token2 = prefs.getString(NOTIFICATION_T2, defV); + _token1 = notification_t1->get(); + _token2 = notification_t2->get(); _port = PUSHOVERPORT; _serveraddress = PUSHOVERSERVER; break; case ESP_LINE_NOTIFICATION: - _token1 = prefs.getString(NOTIFICATION_T1, defV); + _token1 = notification_t1->get(); _port = LINEPORT; _serveraddress = LINESERVER; break; case ESP_EMAIL_NOTIFICATION: - _token1 = base64::encode(prefs.getString(NOTIFICATION_T1, defV)); - _token2 = base64::encode(prefs.getString(NOTIFICATION_T2, defV)); - //log_d("%s",Settings_ESP3D::read_string(ESP_NOTIFICATION_TOKEN1)); - //log_d("%s",Settings_ESP3D::read_string(ESP_NOTIFICATION_TOKEN2)); + _token1 = base64::encode(notification_t1->get()); + _token2 = base64::encode(notification_t2->get()); if (!getEmailFromSettings() || !getPortFromSettings() || !getServerAddressFromSettings()) { - prefs.end(); return false; } break; default: - prefs.end(); return false; break; } - prefs.end(); + bool res = true; if (WiFi.getMode() != WIFI_STA) res = false; if (!res) diff --git a/Grbl_Esp32/nuts_bolts.cpp b/Grbl_Esp32/nuts_bolts.cpp index 19345f5b..72e86230 100644 --- a/Grbl_Esp32/nuts_bolts.cpp +++ b/Grbl_Esp32/nuts_bolts.cpp @@ -36,8 +36,8 @@ // Scientific notation is officially not supported by g-code, and the 'E' character may // be a g-code word on some CNC systems. So, 'E' notation will not be recognized. // NOTE: Thanks to Radu-Eosif Mihailescu for identifying the issues with using strtod(). -uint8_t read_float(char* line, uint8_t* char_counter, float* float_ptr) { - char* ptr = line + *char_counter; +uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr) { + const char* ptr = line + *char_counter; unsigned char c; // Grab first character and increment pointer. No spaces assumed in line. c = *ptr++; @@ -135,12 +135,26 @@ float convert_delta_vector_to_unit_vector(float* vector) { return (magnitude); } -float limit_value_by_axis_maximum(float* max_value, float* unit_vec) { +float limit_acceleration_by_axis_maximum(float* unit_vec) { uint8_t idx; float limit_value = SOME_LARGE_VALUE; for (idx = 0; idx < N_AXIS; idx++) { if (unit_vec[idx] != 0) // Avoid divide by zero. - limit_value = MIN(limit_value, fabs(max_value[idx] / unit_vec[idx])); + limit_value = MIN(limit_value, fabs(axis_settings[idx]->acceleration->get() / unit_vec[idx])); + } + // The acceleration setting is stored and displayed in units of mm/sec^2, + // but used in units of mm/min^2. It suffices to perform the conversion once on + // exit, since the limit computation above is independent of units - it simply + // finds the smallest value. + return (limit_value * SEC_PER_MIN_SQ); +} + +float limit_rate_by_axis_maximum(float* unit_vec) { + uint8_t idx; + float limit_value = SOME_LARGE_VALUE; + for (idx = 0; idx < N_AXIS; idx++) { + if (unit_vec[idx] != 0) // Avoid divide by zero. + limit_value = MIN(limit_value, fabs(axis_settings[idx]->max_rate->get() / unit_vec[idx])); } return (limit_value); } diff --git a/Grbl_Esp32/nuts_bolts.h b/Grbl_Esp32/nuts_bolts.h index b31e2f9d..19ffa2e6 100644 --- a/Grbl_Esp32/nuts_bolts.h +++ b/Grbl_Esp32/nuts_bolts.h @@ -38,6 +38,18 @@ #define B_AXIS 4 #define C_AXIS 5 +#define MAX_AXES 6 +#define MAX_GANGED 2 + +#define PRIMARY_MOTOR 0 +#define GANGED_MOTOR 1 + +#define X2_AXIS (X_AXIS + MAX_AXES) +#define Y2_AXIS (Y_AXIS + MAX_AXES) +#define Z2_AXIS (Z_AXIS + MAX_AXES) +#define A2_AXIS (A_AXIS + MAX_AXES) +#define B2_AXIS (B_AXIS + MAX_AXES) +#define C2_AXIS (C_AXIS + MAX_AXES) // CoreXY motor assignments. DO NOT ALTER. // NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations. @@ -73,7 +85,7 @@ // Read a floating point value from a string. Line points to the input buffer, char_counter // is the indexer pointing to the current character of the line, while float_ptr is // a pointer to the result variable. Returns true when it succeeds -uint8_t read_float(char* line, uint8_t* char_counter, float* float_ptr); +uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr); // Non-blocking delay function used for general operation and suspend features. void delay_sec(float seconds, uint8_t mode); @@ -85,13 +97,15 @@ void delay_ms(uint16_t ms); float hypot_f(float x, float y); float convert_delta_vector_to_unit_vector(float* vector); -float limit_value_by_axis_maximum(float* max_value, float* unit_vec); +float limit_acceleration_by_axis_maximum(float* unit_vec); +float limit_rate_by_axis_maximum(float* unit_vec); float mapConstrain(float x, float in_min, float in_max, float out_min, float out_max); float map_float(float x, float in_min, float in_max, float out_min, float out_max); uint32_t map_uint32_t(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out_min, uint32_t out_max); float constrain_float(float in, float min, float max); bool char_is_numeric(char value); +char* trim(char* value); template void swap(T& a, T& b) { T c(a); a = b; b = c; diff --git a/Grbl_Esp32/planner.cpp b/Grbl_Esp32/planner.cpp index 16130ab5..50d2bcde 100644 --- a/Grbl_Esp32/planner.cpp +++ b/Grbl_Esp32/planner.cpp @@ -304,8 +304,8 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) { #endif } else memcpy(position_steps, pl.position, sizeof(pl.position)); #ifdef COREXY - target_steps[A_MOTOR] = lround(target[A_MOTOR] * settings.steps_per_mm[A_MOTOR]); - target_steps[B_MOTOR] = lround(target[B_MOTOR] * settings.steps_per_mm[B_MOTOR]); + target_steps[A_MOTOR] = lround(target[A_MOTOR] * axis_settings[A_MOTOR]->steps_per_mm->get()); + target_steps[B_MOTOR] = lround(target[B_MOTOR] * axis_settings[B_MOTOR]->steps_per_mm->get()); block->steps[A_MOTOR] = labs((target_steps[X_AXIS] - position_steps[X_AXIS]) + (target_steps[Y_AXIS] - position_steps[Y_AXIS])); block->steps[B_MOTOR] = labs((target_steps[X_AXIS] - position_steps[X_AXIS]) - (target_steps[Y_AXIS] - position_steps[Y_AXIS])); #endif @@ -315,21 +315,21 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) { // NOTE: Computes true distance from converted step values. #ifdef COREXY if (!(idx == A_MOTOR) && !(idx == B_MOTOR)) { - target_steps[idx] = lround(target[idx] * settings.steps_per_mm[idx]); + target_steps[idx] = lround(target[idx] * axis_settings[idx]->steps_per_mm->get()); block->steps[idx] = labs(target_steps[idx] - position_steps[idx]); } block->step_event_count = MAX(block->step_event_count, block->steps[idx]); if (idx == A_MOTOR) - delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] + target_steps[Y_AXIS] - position_steps[Y_AXIS]) / settings.steps_per_mm[idx]; + delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] + target_steps[Y_AXIS] - position_steps[Y_AXIS]) / axis_settings[idx]->steps_per_mm->get(); else if (idx == B_MOTOR) - delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] - target_steps[Y_AXIS] + position_steps[Y_AXIS]) / settings.steps_per_mm[idx]; + delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] - target_steps[Y_AXIS] + position_steps[Y_AXIS]) / axis_settings[idx]->steps_per_mm->get(); else - delta_mm = (target_steps[idx] - position_steps[idx]) / settings.steps_per_mm[idx]; + delta_mm = (target_steps[idx] - position_steps[idx]) / axis_settings[idx]->steps_per_mm->get(); #else - target_steps[idx] = lround(target[idx] * settings.steps_per_mm[idx]); + target_steps[idx] = lround(target[idx] * axis_settings[idx]->steps_per_mm->get()); block->steps[idx] = labs(target_steps[idx] - position_steps[idx]); block->step_event_count = MAX(block->step_event_count, block->steps[idx]); - delta_mm = (target_steps[idx] - position_steps[idx]) / settings.steps_per_mm[idx]; + delta_mm = (target_steps[idx] - position_steps[idx]) / axis_settings[idx]->steps_per_mm->get(); #endif unit_vec[idx] = delta_mm; // Store unit vector numerator // Set direction bits. Bit enabled always means direction is negative. @@ -342,8 +342,8 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) { // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); - block->acceleration = limit_value_by_axis_maximum(settings.acceleration, unit_vec); - block->rapid_rate = limit_value_by_axis_maximum(settings.max_rate, unit_vec); + block->acceleration = limit_acceleration_by_axis_maximum(unit_vec); + block->rapid_rate = limit_rate_by_axis_maximum(unit_vec); // Store programmed rate. if (block->condition & PL_COND_FLAG_RAPID_MOTION) block->programmed_rate = block->rapid_rate; else { @@ -394,10 +394,10 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) { block->max_junction_speed_sqr = SOME_LARGE_VALUE; } else { convert_delta_vector_to_unit_vector(junction_unit_vec); - float junction_acceleration = limit_value_by_axis_maximum(settings.acceleration, junction_unit_vec); + float junction_acceleration = limit_acceleration_by_axis_maximum(junction_unit_vec); float sin_theta_d2 = sqrt(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive. block->max_junction_speed_sqr = MAX(MINIMUM_JUNCTION_SPEED * MINIMUM_JUNCTION_SPEED, - (junction_acceleration * settings.junction_deviation * sin_theta_d2) / (1.0 - sin_theta_d2)); + (junction_acceleration * junction_deviation->get() * sin_theta_d2) / (1.0 - sin_theta_d2)); } } } diff --git a/Grbl_Esp32/print.cpp b/Grbl_Esp32/print.cpp index 034f2c61..8fec2847 100644 --- a/Grbl_Esp32/print.cpp +++ b/Grbl_Esp32/print.cpp @@ -120,7 +120,7 @@ void printFloat(float n, uint8_t decimal_places) { // - CoordValue: Handles all position or coordinate values in inches or mm reporting. // - RateValue: Handles feed rate and current velocity in inches or mm reporting. void printFloat_CoordValue(float n) { - if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES)) + if (report_inches->get()) printFloat(n * INCH_PER_MM, N_DECIMAL_COORDVALUE_INCH); else printFloat(n, N_DECIMAL_COORDVALUE_MM); diff --git a/Grbl_Esp32/probe.cpp b/Grbl_Esp32/probe.cpp index 5a409123..d53d2bc6 100644 --- a/Grbl_Esp32/probe.cpp +++ b/Grbl_Esp32/probe.cpp @@ -46,7 +46,7 @@ void probe_init() { // and the probing cycle modes for toward-workpiece/away-from-workpiece. void probe_configure_invert_mask(uint8_t is_probe_away) { probe_invert_mask = 0; // Initialize as zero. - if (bit_isfalse(settings.flags, BITFLAG_INVERT_PROBE_PIN)) probe_invert_mask ^= PROBE_MASK; + if (probe_invert->get()) probe_invert_mask ^= PROBE_MASK; if (is_probe_away) probe_invert_mask ^= PROBE_MASK; } diff --git a/Grbl_Esp32/protocol.cpp b/Grbl_Esp32/protocol.cpp index 982c7da9..6a21420c 100644 --- a/Grbl_Esp32/protocol.cpp +++ b/Grbl_Esp32/protocol.cpp @@ -23,22 +23,70 @@ */ #include "grbl.h" -#include "config.h" -#include "commands.h" -#include "espresponse.h" - -// Define line flags. Includes comment type tracking and line overflow detection. -#define LINE_FLAG_OVERFLOW bit(0) -#define LINE_FLAG_COMMENT_PARENTHESES bit(1) -#define LINE_FLAG_COMMENT_SEMICOLON bit(2) -#define LINE_FLAG_BRACKET bit(3) // square bracket for WebUI commands - - -static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. -static char comment[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. static void protocol_exec_rt_suspend(); +static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. +static char comment[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. +static uint8_t line_flags = 0; +static uint8_t char_counter = 0; +static uint8_t comment_char_counter = 0; + +typedef struct { + char buffer[LINE_BUFFER_SIZE]; + int len; + int line_number; +} client_line_t; +client_line_t client_lines[CLIENT_COUNT]; + +void empty_line(uint8_t client) +{ + client_line_t* cl = &client_lines[client]; + cl->len = 0; + cl->buffer[0] = '\0'; +} +err_t add_char_to_line(char c, uint8_t client) +{ + client_line_t* cl = &client_lines[client]; + // Simple editing for interactive input + if (c == '\b') { + // Backspace erases + if (cl->len) { + --cl->len; + cl->buffer[cl->len] = '\0'; + } + return STATUS_OK; + } + if (cl->len == (LINE_BUFFER_SIZE - 1)) { + return STATUS_OVERFLOW; + } + if (c == '\r' || c == '\n') { + cl->len = 0; + cl->line_number++; + return STATUS_EOL; + } + cl->buffer[cl->len++] = c; + cl->buffer[cl->len] = '\0'; + return STATUS_OK; +} + +err_t execute_line(char* line, uint8_t client, auth_t auth_level) +{ + err_t result = STATUS_OK; + // Empty or comment line. For syncing purposes. + if (line[0] == 0) { + return STATUS_OK; + } + // Grbl '$' or WebUI '[ESPxxx]' system command + if (line[0] == '$' || line[0] == '[') { + return system_execute_line(line, client, auth_level); + } + // Everything else is gcode. Block if in alarm or jog mode. + if (sys.state & (STATE_ALARM | STATE_JOG)) { + return STATUS_SYSTEM_GC_LOCK; + } + return gc_execute_line(line, client); +} /* GRBL PRIMARY LOOP: @@ -47,7 +95,7 @@ void protocol_main_loop() { //uint8_t client = CLIENT_SERIAL; // default client // Perform some machine checks to make sure everything is good to go. #ifdef CHECK_LIMITS_AT_INIT - if (bit_istrue(settings.flags, BITFLAG_HARD_LIMIT_ENABLE)) { + if (hard_limits->get()) { if (limits_get_state()) { sys.state = STATE_ALARM; // Ensure alarm state is active. report_feedback_message(MESSAGE_CHECK_LIMITS); @@ -74,15 +122,12 @@ void protocol_main_loop() { // Primary loop! Upon a system abort, this exits back to main() to reset the system. // This is also where Grbl idles while waiting for something to do. // --------------------------------------------------------------------------------- - uint8_t line_flags = 0; - uint8_t char_counter = 0; - uint8_t comment_char_counter = 0; uint8_t c; for (;;) { #ifdef ENABLE_SD_CARD if (SD_ready_next) { char fileLine[255]; - if (readFileLine(fileLine)) { + if (readFileLine(fileLine, 255)) { SD_ready_next = false; report_status_message(gc_execute_line(fileLine, SD_client), SD_client); } else { @@ -93,102 +138,34 @@ void protocol_main_loop() { } } #endif - // Process one line of incoming serial data, as the data becomes available. Performs an - // initial filtering by removing spaces and comments and capitalizing all letters. + // Receive one line of incoming serial data, as the data becomes available. + // Filtering, if necessary, is done later in gc_execute_line(), so the + // filtering is the same with serial and file input. uint8_t client = CLIENT_SERIAL; + char* line; for (client = 0; client < CLIENT_COUNT; client++) { while ((c = serial_read(client)) != SERIAL_NO_DATA) { - if ((c == '\n') || (c == '\r')) { // End of line reached - protocol_execute_realtime(); // Runtime command check point. - if (sys.abort) return; // Bail to calling function upon system abort - line[char_counter] = 0; // Set string termination character. -#ifdef REPORT_ECHO_LINE_RECEIVED - report_echo_line_received(line, client); + err_t res = add_char_to_line(c, client); + switch (res) { + case STATUS_OK: + break; + case STATUS_EOL: + protocol_execute_realtime(); // Runtime command check point. + if (sys.abort) { + return; // Bail to calling function upon system abort + } + line = client_lines[client].buffer; +#ifdef REPORT_ECHO_RAW_LINE_RECEIVED + report_echo_line_received(line, client); #endif - // Direct and execute one line of formatted input, and report status of execution. - if (line_flags & LINE_FLAG_OVERFLOW) { - // Report line overflow error. + // auth_level can be upgraded by supplying a password on the command line + report_status_message(execute_line(line, client, LEVEL_GUEST), client); + empty_line(client); + break; + case STATUS_OVERFLOW: report_status_message(STATUS_OVERFLOW, client); - } else if (line[0] == 0) { - // Empty or comment line. For syncing purposes. - report_status_message(STATUS_OK, client); - } else if (line[0] == '$') { - // Grbl '$' system command - report_status_message(system_execute_line(line, client), client); - } else if (line[0] == '[') { - int cmd = 0; - String cmd_params; - if (COMMANDS::check_command(line, &cmd, cmd_params)) { - ESPResponseStream espresponse(client, true); - if (!COMMANDS::execute_internal_command(cmd, cmd_params, LEVEL_GUEST, &espresponse)) - report_status_message(STATUS_GCODE_UNSUPPORTED_COMMAND, CLIENT_ALL); - } else grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Unknow Command...%s", line); - } else if (sys.state & (STATE_ALARM | STATE_JOG)) { - // Everything else is gcode. Block if in alarm or jog mode. - report_status_message(STATUS_SYSTEM_GC_LOCK, client); - } else { - // Parse and execute g-code block - report_status_message(gc_execute_line(line, client), client); - } - // Reset tracking data for next line. - line_flags = 0; - char_counter = 0; - comment_char_counter = 0; - } else { - if (line_flags) { - if (line_flags & LINE_FLAG_BRACKET) // in bracket mode all characters are accepted - line[char_counter++] = c; - // Throw away all (except EOL) comment characters and overflow characters. - if (c == ')') { - // End of '()' comment. Resume line allowed. - if (line_flags & LINE_FLAG_COMMENT_PARENTHESES) { - line_flags &= ~(LINE_FLAG_COMMENT_PARENTHESES); - comment[comment_char_counter] = 0; // null terminate - report_gcode_comment(comment); - } - } - if (line_flags & LINE_FLAG_COMMENT_PARENTHESES) // capture all characters into a comment buffer - comment[comment_char_counter++] = c; - } else { - if (c <= ' ') { - // Throw away whitepace and control characters - } - /* - else if (c == '/') { - // Block delete NOT SUPPORTED. Ignore character. - // NOTE: If supported, would simply need to check the system if block delete is enabled. - } - */ - else if (c == '(') { - // Enable comments flag and ignore all characters until ')' or EOL. - // NOTE: This doesn't follow the NIST definition exactly, but is good enough for now. - // In the future, we could simply remove the items within the comments, but retain the - // comment control characters, so that the g-code parser can error-check it. - line_flags |= LINE_FLAG_COMMENT_PARENTHESES; - comment_char_counter = 0; - } else if (c == ';') { - // NOTE: ';' comment to EOL is a LinuxCNC definition. Not NIST. - line_flags |= LINE_FLAG_COMMENT_SEMICOLON; - } else if (c == '[') { - // For ESP3D bracket commands like [ESP100]pwd= - // prevents spaces being striped and converting to uppercase - line_flags |= LINE_FLAG_BRACKET; - line[char_counter++] = c; // capture this character - // TODO: Install '%' feature - } else if (c == '%') { - // Program start-end percent sign NOT SUPPORTED. - // NOTE: This maybe installed to tell Grbl when a program is running vs manual input, - // where, during a program, the system auto-cycle start will continue to execute - // everything until the next '%' sign. This will help fix resuming issues with certain - // functions that empty the planner buffer to execute its task on-time. - } else if (char_counter >= (LINE_BUFFER_SIZE - 1)) { - // Detect line buffer overflow and set flag. - line_flags |= LINE_FLAG_OVERFLOW; - } else if (c >= 'a' && c <= 'z') // Upcase lowercase - line[char_counter++] = c - 'a' + 'A'; - else - line[char_counter++] = c; - } + empty_line(client); + break; } } // while serial read } // for clients @@ -197,11 +174,14 @@ void protocol_main_loop() { // completed. In either case, auto-cycle start, if enabled, any queued moves. protocol_auto_cycle_start(); protocol_execute_realtime(); // Runtime command check point. - if (sys.abort) return; // Bail to main() program loop to reset system. + if (sys.abort) { + return; // Bail to main() program loop to reset system. + } // check to see if we should disable the stepper drivers ... esp32 work around for disable in main loop. if (stepper_idle) { - if (esp_timer_get_time() > stepper_idle_counter) - set_stepper_disable(true); + if (esp_timer_get_time() > stepper_idle_counter) { + motors_set_disable(true); + } } } return; /* Never reached */ @@ -544,7 +524,7 @@ static void protocol_exec_rt_suspend() { restore_spindle_speed = block->spindle_speed; } #ifdef DISABLE_LASER_DURING_HOLD - if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) + if (laser_mode->get()) system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); #endif @@ -574,14 +554,14 @@ static void protocol_exec_rt_suspend() { // current location not exceeding the parking target location, and laser mode disabled. // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if ((bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)) && - (parking_target[PARKING_AXIS] < PARKING_TARGET) && - bit_isfalse(settings.flags, BITFLAG_LASER_MODE) && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { + if (homing_enable->get() && + (parking_target[PARKING_AXIS] < PARKING_TARGET) && + laser_mode->get() && + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { #else - if ((bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)) && + if (homing_enable->get() && (parking_target[PARKING_AXIS] < PARKING_TARGET) && - bit_isfalse(settings.flags, BITFLAG_LASER_MODE)) { + laser_mode->get()) { #endif // Retract spindle by pullout distance. Ensure retraction motion moves away from // the workpiece and waypoint motion doesn't exceed the parking target location. @@ -634,10 +614,10 @@ static void protocol_exec_rt_suspend() { // Execute fast restore motion to the pull-out position. Parking requires homing enabled. // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if (((settings.flags & (BITFLAG_HOMING_ENABLE | BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) && + if (homing_enable->get() && !laser_mode->get()) && (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { #else - if ((settings.flags & (BITFLAG_HOMING_ENABLE | BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) { + if (homing_enable->get() && !laser_mode->get()) { #endif // Check to ensure the motion doesn't move below pull-out position. if (parking_target[PARKING_AXIS] <= PARKING_TARGET) { @@ -651,7 +631,7 @@ static void protocol_exec_rt_suspend() { if (gc_state.modal.spindle != SPINDLE_DISABLE) { // Block if safety door re-opened during prior restore actions. if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { - if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) { + if (laser_mode->get()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); } else { @@ -671,10 +651,10 @@ static void protocol_exec_rt_suspend() { #ifdef PARKING_ENABLE // Execute slow plunge motion from pull-out position to resume position. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if (((settings.flags & (BITFLAG_HOMING_ENABLE | BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) && + if (homing_enable->get() && !laser_mode->get()) && (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { #else - if ((settings.flags & (BITFLAG_HOMING_ENABLE | BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) { + if (homing_enable->get() && !laser_mode->get()) { #endif // Block if safety door re-opened during prior restore actions. if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { @@ -710,7 +690,7 @@ static void protocol_exec_rt_suspend() { } else if (sys.spindle_stop_ovr & (SPINDLE_STOP_OVR_RESTORE | SPINDLE_STOP_OVR_RESTORE_CYCLE)) { if (gc_state.modal.spindle != SPINDLE_DISABLE) { report_feedback_message(MESSAGE_SPINDLE_RESTORE); - if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) { + if (laser_mode->get()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); } else diff --git a/Grbl_Esp32/report.cpp b/Grbl_Esp32/report.cpp index 988a837e..cb027a57 100644 --- a/Grbl_Esp32/report.cpp +++ b/Grbl_Esp32/report.cpp @@ -47,7 +47,9 @@ */ #include "grbl.h" - +#ifdef REPORT_HEAP +EspClass esp; +#endif #define DEFAULTBUFFERSIZE 64 // this is a generic send function that everything should use, so interfaces could be added (Bluetooth, etc) @@ -151,10 +153,10 @@ static void report_util_axis_values(float* axis_value, char* rpt) { char axisVal[20]; float unit_conv = 1.0; // unit conversion multiplier..default is mm rpt[0] = '\0'; - if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES)) + if (report_inches->get()) unit_conv = 1.0 / MM_PER_INCH; for (idx = 0; idx < N_AXIS; idx++) { - if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES)) + if (report_inches->get()) sprintf(axisVal, "%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 @@ -272,81 +274,10 @@ void report_init_message(uint8_t client) { // Grbl help message void report_grbl_help(uint8_t client) { - grbl_send(client, "[HLP:$$ $+ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H $F ~ ! ? ctrl-x]\r\n"); + grbl_send(client, "[HLP:$$ $+ $# $S $L $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H $F $E=err ~ ! ? ctrl-x]\r\n"); } -// Grbl global settings print out. -// NOTE: The numbering scheme here must correlate to storing in settings.c -// Extended setting will be displayed if force_extended is true or #ifdef SHOW_EXTENDED_SETTINGS -void report_grbl_settings(uint8_t client, uint8_t show_extended) { - // Print Grbl settings. - char setting[20]; - char rpt[1000]; -#ifdef SHOW_EXTENDED_SETTINGS - show_extended = true; -#endif - rpt[0] = '\0'; - sprintf(setting, "$0=%d\r\n", settings.pulse_microseconds); strcat(rpt, setting); - sprintf(setting, "$1=%d\r\n", settings.stepper_idle_lock_time); strcat(rpt, setting); - sprintf(setting, "$2=%d\r\n", settings.step_invert_mask); strcat(rpt, setting); - sprintf(setting, "$3=%d\r\n", settings.dir_invert_mask); strcat(rpt, setting); - sprintf(setting, "$4=%d\r\n", bit_istrue(settings.flags, BITFLAG_INVERT_ST_ENABLE)); strcat(rpt, setting); - sprintf(setting, "$5=%d\r\n", bit_istrue(settings.flags, BITFLAG_INVERT_LIMIT_PINS)); strcat(rpt, setting); - sprintf(setting, "$6=%d\r\n", bit_istrue(settings.flags, BITFLAG_INVERT_PROBE_PIN)); strcat(rpt, setting); - sprintf(setting, "$10=%d\r\n", settings.status_report_mask); strcat(rpt, setting); - sprintf(setting, "$11=%4.3f\r\n", settings.junction_deviation); strcat(rpt, setting); - sprintf(setting, "$12=%4.3f\r\n", settings.arc_tolerance); strcat(rpt, setting); - sprintf(setting, "$13=%d\r\n", bit_istrue(settings.flags, BITFLAG_REPORT_INCHES)); strcat(rpt, setting); - sprintf(setting, "$20=%d\r\n", bit_istrue(settings.flags, BITFLAG_SOFT_LIMIT_ENABLE)); strcat(rpt, setting); - sprintf(setting, "$21=%d\r\n", bit_istrue(settings.flags, BITFLAG_HARD_LIMIT_ENABLE)); strcat(rpt, setting); - sprintf(setting, "$22=%d\r\n", bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)); strcat(rpt, setting); - sprintf(setting, "$23=%d\r\n", settings.homing_dir_mask); strcat(rpt, setting); - sprintf(setting, "$24=%4.3f\r\n", settings.homing_feed_rate); strcat(rpt, setting); - sprintf(setting, "$25=%4.3f\r\n", settings.homing_seek_rate); strcat(rpt, setting); - sprintf(setting, "$26=%d\r\n", settings.homing_debounce_delay); strcat(rpt, setting); - sprintf(setting, "$27=%4.3f\r\n", settings.homing_pulloff); strcat(rpt, setting); - sprintf(setting, "$30=%4.3f\r\n", settings.rpm_max); strcat(rpt, setting); - sprintf(setting, "$31=%4.3f\r\n", settings.rpm_min); strcat(rpt, setting); - sprintf(setting, "$32=%d\r\n", bit_istrue(settings.flags, BITFLAG_LASER_MODE)); strcat(rpt, setting); - - if (show_extended) { - sprintf(setting, "$33=%5.3f\r\n", settings.spindle_pwm_freq); strcat(rpt, setting); - sprintf(setting, "$34=%3.3f\r\n", settings.spindle_pwm_off_value); strcat(rpt, setting); - sprintf(setting, "$35=%3.3f\r\n", settings.spindle_pwm_min_value); strcat(rpt, setting); - sprintf(setting, "$36=%3.3f\r\n", settings.spindle_pwm_max_value); strcat(rpt, setting); - for (uint8_t index = 0; index < USER_SETTING_COUNT; index++) { - sprintf(setting, "$%d=%d\r\n", 80 + index, settings.machine_int16[index]); strcat(rpt, setting); - } - for (uint8_t index = 0; index < USER_SETTING_COUNT; index++) { - sprintf(setting, "$%d=%5.3f\r\n", 90 + index, settings.machine_float[index]); strcat(rpt, setting); - } - } - // Print axis settings - uint8_t idx, set_idx; - uint8_t val = AXIS_SETTINGS_START_VAL; - for (set_idx = 0; set_idx < AXIS_N_SETTINGS; set_idx++) { - for (idx = 0; idx < N_AXIS; idx++) { - switch (set_idx) { - case 0: sprintf(setting, "$%d=%4.3f\r\n", val + idx, settings.steps_per_mm[idx]); strcat(rpt, setting); break; - case 1: sprintf(setting, "$%d=%4.3f\r\n", val + idx, settings.max_rate[idx]); strcat(rpt, setting); break; - case 2: sprintf(setting, "$%d=%4.3f\r\n", val + idx, settings.acceleration[idx] / (60 * 60)); strcat(rpt, setting); break; - case 3: sprintf(setting, "$%d=%4.3f\r\n", val + idx, -settings.max_travel[idx]); strcat(rpt, setting); break; - case 4: if (show_extended) {sprintf(setting, "$%d=%4.3f\r\n", val + idx, settings.current[idx]); strcat(rpt, setting);} break; - case 5: if (show_extended) {sprintf(setting, "$%d=%4.3f\r\n", val + idx, settings.hold_current[idx]); strcat(rpt, setting);} break; - case 6: if (show_extended) {sprintf(setting, "$%d=%d\r\n", val + idx, settings.microsteps[idx]); strcat(rpt, setting);} break; - case 7: if (show_extended) {sprintf(setting, "$%d=%d\r\n", val + idx, settings.stallguard[idx]); strcat(rpt, setting);} break; - } - } - val += AXIS_SETTINGS_INCREMENT; - } - grbl_send(client, rpt); -} - - - - - // Prints current probe parameters. Upon a probe command, these parameters are updated upon a // successful probe or upon a failed probe with the G38.3 without errors command (if supported). @@ -401,7 +332,7 @@ void report_ngc_parameters(uint8_t client) { strcat(ngc_rpt, temp); strcat(ngc_rpt, "]\r\n"); strcat(ngc_rpt, "[TLO:"); // Print tool length offset value - if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES)) + if (report_inches->get()) sprintf(temp, "%4.3f]\r\n", gc_state.tool_length_offset * INCH_PER_MM); else sprintf(temp, "%4.3f]\r\n", gc_state.tool_length_offset); @@ -463,10 +394,7 @@ void report_gcode_modes(uint8_t client) { sprintf(temp, " T%d", gc_state.tool); strcat(modes_rpt, temp); - if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES)) - sprintf(temp, " F%.1f", gc_state.feed_rate); - else - sprintf(temp, " F%.0f", gc_state.feed_rate); + sprintf(temp, report_inches->get() ? " F%.1f" : " F%.0f", gc_state.feed_rate); strcat(modes_rpt, temp); sprintf(temp, " S%4.3f", gc_state.spindle_speed); strcat(modes_rpt, temp); @@ -477,11 +405,11 @@ void report_gcode_modes(uint8_t client) { // Prints specified startup line -void report_startup_line(uint8_t n, char* line, uint8_t client) { +void report_startup_line(uint8_t n, const char* line, uint8_t client) { grbl_sendf(client, "$N%d=%s\r\n", n, line); // OK to send to all } -void report_execute_startup_message(char* line, uint8_t status_code, uint8_t client) { +void report_execute_startup_message(const char* line, uint8_t status_code, uint8_t client) { grbl_sendf(client, ">%s:", line); // OK to send to all report_status_message(status_code, client); } @@ -558,9 +486,6 @@ void report_build_info(char* line, uint8_t client) { #endif } - - - // Prints the character string line Grbl has received from the user, which has been pre-parsed, // and has been sent into protocol_execute_line() routine to be executed by Grbl. void report_echo_line_received(char* line, uint8_t client) { @@ -615,18 +540,18 @@ void report_realtime_status(uint8_t client) { case STATE_SLEEP: strcat(status, "Sleep"); break; } float wco[N_AXIS]; - if (bit_isfalse(settings.status_report_mask, BITFLAG_RT_STATUS_POSITION_TYPE) || + if (bit_isfalse(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE) || (sys.report_wco_counter == 0)) { for (idx = 0; idx < N_AXIS; idx++) { // Apply work coordinate offsets and tool length offset to current position. wco[idx] = gc_state.coord_system[idx] + gc_state.coord_offset[idx]; if (idx == TOOL_LENGTH_OFFSET_AXIS) wco[idx] += gc_state.tool_length_offset; - if (bit_isfalse(settings.status_report_mask, BITFLAG_RT_STATUS_POSITION_TYPE)) + if (bit_isfalse(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) print_position[idx] -= wco[idx]; } } // Report machine position - if (bit_istrue(settings.status_report_mask, BITFLAG_RT_STATUS_POSITION_TYPE)) + if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) strcat(status, "|MPos:"); else { #ifdef USE_FWD_KINEMATIC @@ -638,7 +563,7 @@ void report_realtime_status(uint8_t client) { strcat(status, temp); // Returns planner and serial read buffer states. #ifdef REPORT_FIELD_BUFFER_STATE - if (bit_istrue(settings.status_report_mask, BITFLAG_RT_STATUS_BUFFER_STATE)) { + if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_BUFFER_STATE)) { int bufsize = DEFAULTBUFFERSIZE; #if defined (ENABLE_WIFI) && defined(ENABLE_TELNET) if (client == CLIENT_TELNET) @@ -671,7 +596,7 @@ void report_realtime_status(uint8_t client) { #endif // Report realtime feed speed #ifdef REPORT_FIELD_CURRENT_FEED_SPEED - if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES)) + if (report_inches->get()) sprintf(temp, "|FS:%.1f,%d", st_get_realtime_rate()/ MM_PER_INCH, sys.spindle_speed); else sprintf(temp, "|FS:%.0f,%d", st_get_realtime_rate(), sys.spindle_speed); @@ -750,6 +675,10 @@ void report_realtime_status(uint8_t client) { sd_get_current_filename(temp); strcat(status, temp); } +#endif +#ifdef REPORT_HEAP + sprintf(temp, "|Heap:%d", esp.getHeapSize()); + strcat(status, temp); #endif strcat(status, ">\r\n"); grbl_send(client, status); @@ -817,11 +746,3 @@ char report_get_axis_letter(uint8_t axis) { return '?'; } } - -// used to report the pin nhumber or -1 for undefined. -int16_t report_pin_number(uint8_t pin_number) { - if (pin_number == UNDEFINED_PIN) - return -1; - else - return (int16_t)pin_number; -} \ No newline at end of file diff --git a/Grbl_Esp32/report.h b/Grbl_Esp32/report.h index d6592f56..51502392 100644 --- a/Grbl_Esp32/report.h +++ b/Grbl_Esp32/report.h @@ -21,8 +21,6 @@ #ifndef report_h #define report_h -#include "grbl.h" - // Define Grbl status codes. Valid values (0-255) #define STATUS_OK 0 #define STATUS_EXPECTED_COMMAND_LETTER 1 @@ -69,10 +67,27 @@ #define STATUS_SD_FAILED_OPEN_DIR 62 // SD card failed to open directory #define STATUS_SD_DIR_NOT_FOUND 63 // SD Card directory not found #define STATUS_SD_FILE_EMPTY 64 // SD Card directory not found +#define STATUS_SD_FILE_NOT_FOUND 65 // SD Card file not found +#define STATUS_SD_FAILED_OPEN_FILE 66 // SD card failed to open file +#define STATUS_SD_FAILED_BUSY 67 // SD card is busy +#define STATUS_SD_FAILED_DEL_DIR 68 +#define STATUS_SD_FAILED_DEL_FILE 69 #define STATUS_BT_FAIL_BEGIN 70 // Bluetooth failed to start +#define STATUS_WIFI_FAIL_BEGIN 71 // WiFi failed to start +#define STATUS_NUMBER_RANGE 80 // Setting number range problem +#define STATUS_INVALID_VALUE 81 // Setting string problem +#define STATUS_MESSAGE_FAILED 90 + +#define STATUS_NVS_SET_FAILED 100 + +#define STATUS_AUTHENTICATION_FAILED 110 +#define STATUS_EOL 111 + +typedef uint8_t err_t; // For status codes +const char* errorString(err_t errorNumber); // Define Grbl alarm codes. Valid values (1-255). 0 is reserved. #define ALARM_HARD_LIMIT_ERROR EXEC_ALARM_HARD_LIMIT @@ -158,8 +173,8 @@ void report_ngc_parameters(uint8_t client); void report_gcode_modes(uint8_t client); // Prints startup line when requested and executed. -void report_startup_line(uint8_t n, char* line, uint8_t client); -void report_execute_startup_message(char* line, uint8_t status_code, uint8_t client); +void report_startup_line(uint8_t n, const char* line, uint8_t client); +void report_execute_startup_message(const char* line, uint8_t status_code, uint8_t client); // Prints build info and user info void report_build_info(char* line, uint8_t client); @@ -175,6 +190,5 @@ void report_machine_type(uint8_t client); void report_hex_msg(char* buf, const char *prefix, int len); char report_get_axis_letter(uint8_t axis); -int16_t report_pin_number(uint8_t pin_number); #endif diff --git a/Grbl_Esp32/serial.cpp b/Grbl_Esp32/serial.cpp index ea883da1..2e4f77c8 100644 --- a/Grbl_Esp32/serial.cpp +++ b/Grbl_Esp32/serial.cpp @@ -56,7 +56,6 @@ */ #include "grbl.h" -#include "commands.h" portMUX_TYPE myMutex = portMUX_INITIALIZER_UNLOCKED; diff --git a/Grbl_Esp32/serial2socket.cpp b/Grbl_Esp32/serial2socket.cpp index 1b1176fb..20ffef24 100644 --- a/Grbl_Esp32/serial2socket.cpp +++ b/Grbl_Esp32/serial2socket.cpp @@ -21,8 +21,7 @@ #ifdef ARDUINO_ARCH_ESP32 -//#include "grbl.h" -#include "config.h" +#include "grbl.h" #if defined (ENABLE_WIFI) && defined(ENABLE_HTTP) diff --git a/Grbl_Esp32/servo_axis.cpp b/Grbl_Esp32/servo_axis.cpp index e3366cc1..9d06b86a 100644 --- a/Grbl_Esp32/servo_axis.cpp +++ b/Grbl_Esp32/servo_axis.cpp @@ -267,18 +267,18 @@ void ServoAxis::set_location() { // 5. Apply the calibrarion servo_pulse_min = SERVO_MIN_PULSE; servo_pulse_max = SERVO_MAX_PULSE; - if (bit_istrue(settings.dir_invert_mask, bit(_axis))) // this allows the user to change the direction via settings + 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(settings.dir_invert_mask, bit(_axis))) { // normal direction - min_pulse_cal = 2.0 - (settings.steps_per_mm[_axis] / 100.0); - max_pulse_cal = (settings.max_travel[_axis] / -100.0); + 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 = (settings.steps_per_mm[_axis] / 100.0); - max_pulse_cal = 2.0 - (settings.max_travel[_axis] / -100.0); + 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; @@ -306,27 +306,25 @@ void ServoAxis::disable() { // vebose = true if you want an error sent to serial port bool ServoAxis::_cal_is_valid() { bool settingsOK = true; - if ((settings.steps_per_mm[_axis] < SERVO_CAL_MIN) || (settings.steps_per_mm[_axis] > SERVO_CAL_MAX)) { + 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); - settings.steps_per_mm[_axis] = 100; - write_global_settings(); + 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 - if ((settings.max_travel[_axis] < -SERVO_CAL_MAX) || (settings.max_travel[_axis] > -SERVO_CAL_MIN)) { + 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); - settings.max_travel[_axis] = -100; - write_global_settings(); + char reset_val[] = "-100"; // stored as a negative + axis_settings[_axis]->max_travel->setStringValue(reset_val); } settingsOK = false; } _showError = false; // to show error once - if (! settingsOK) { - write_global_settings(); // they were changed so write them to - } return settingsOK; } diff --git a/Grbl_Esp32/servo_axis.h b/Grbl_Esp32/servo_axis.h index 75d4bb35..14fc5c66 100644 --- a/Grbl_Esp32/servo_axis.h +++ b/Grbl_Esp32/servo_axis.h @@ -54,32 +54,7 @@ #ifndef servo_axis_h #define servo_axis_h - - -// this is the pulse range of a the servo. Typical servos are 0.001 to 0.002 seconds -// some servos have a wider range. You can adjust this here or in the calibration feature -#define SERVO_MIN_PULSE_SEC 0.001 // min pulse in seconds -#define SERVO_MAX_PULSE_SEC 0.002 // max pulse in seconds - -#define SERVO_POSITION_MIN_DEFAULT 0.0 // mm -#define SERVO_POSITION_MAX_DEFAULT 20.0 // mm - -#define SERVO_PULSE_FREQ 50 // 50Hz ...This is a standard analog servo value. Digital ones can repeat faster - -#define SERVO_PULSE_RES_BITS 16 // bits of resolution of PWM (16 is max) -#define SERVO_PULSE_RES_COUNT 65535 // see above TODO...do the math here 2^SERVO_PULSE_RES_BITS - -#define SERVO_TIME_PER_BIT ((1.0 / (float)SERVO_PULSE_FREQ) / ((float)SERVO_PULSE_RES_COUNT) ) // seconds - -#define SERVO_MIN_PULSE (uint16_t)(SERVO_MIN_PULSE_SEC / SERVO_TIME_PER_BIT) // in timer counts -#define SERVO_MAX_PULSE (uint16_t)(SERVO_MAX_PULSE_SEC / SERVO_TIME_PER_BIT) // in timer counts - -#define SERVO_PULSE_RANGE (SERVO_MAX_PULSE-SERVO_MIN_PULSE) - -#define SERVO_CAL_MIN 20.0 // Percent: the minimum allowable calibration value -#define SERVO_CAL_MAX 180.0 // Percent: the maximum allowable calibration value - -#define SERVO_TIMER_INT_FREQ 20.0 // Hz This is the task frequency +#include "Motors/RcServoClass.h" #define SERVO_HOMING_OFF 0 // servo is off during homing #define SERVO_HOMING_TARGET 1 // servo is send to a location during homing diff --git a/Grbl_Esp32/settings.cpp b/Grbl_Esp32/settings.cpp index f97f54bb..3c480adc 100644 --- a/Grbl_Esp32/settings.cpp +++ b/Grbl_Esp32/settings.cpp @@ -24,185 +24,6 @@ #include "grbl.h" -settings_t settings; - -// Method to store startup lines into EEPROM -void settings_store_startup_line(uint8_t n, char* line) { -#ifdef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE - protocol_buffer_synchronize(); // A startup line may contain a motion and be executing. -#endif - uint32_t addr = n * (LINE_BUFFER_SIZE + 1) + EEPROM_ADDR_STARTUP_BLOCK; - memcpy_to_eeprom_with_checksum(addr, (char*)line, LINE_BUFFER_SIZE); -} - -void settings_init() { - EEPROM.begin(EEPROM_SIZE); - if (!read_global_settings()) { - report_status_message(STATUS_SETTING_READ_FAIL, CLIENT_SERIAL); - settings_restore(SETTINGS_RESTORE_ALL); // Force restore all EEPROM data. - report_grbl_settings(CLIENT_SERIAL, false); - } -} - -// Method to restore EEPROM-saved Grbl global settings back to defaults. -void settings_restore(uint8_t restore_flag) { -#if defined(ENABLE_BLUETOOTH) || defined(ENABLE_WIFI) - if (restore_flag & SETTINGS_RESTORE_WIFI_SETTINGS) { -#ifdef ENABLE_WIFI - wifi_config.reset_settings(); -#endif -#ifdef ENABLE_BLUETOOTH - bt_config.reset_settings(); -#endif - } -#endif - if (restore_flag & SETTINGS_RESTORE_DEFAULTS) { - settings.pulse_microseconds = DEFAULT_STEP_PULSE_MICROSECONDS; - settings.stepper_idle_lock_time = DEFAULT_STEPPER_IDLE_LOCK_TIME; - settings.step_invert_mask = DEFAULT_STEPPING_INVERT_MASK; - settings.dir_invert_mask = DEFAULT_DIRECTION_INVERT_MASK; - settings.status_report_mask = DEFAULT_STATUS_REPORT_MASK; - settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION; - settings.arc_tolerance = DEFAULT_ARC_TOLERANCE; - - settings.rpm_max = DEFAULT_SPINDLE_RPM_MAX; - settings.rpm_min = DEFAULT_SPINDLE_RPM_MIN; - settings.spindle_pwm_freq = DEFAULT_SPINDLE_FREQ; // $33 Hz (extended set) - settings.spindle_pwm_off_value = DEFAULT_SPINDLE_OFF_VALUE; // $34 Percent (extended set) - settings.spindle_pwm_min_value = DEFAULT_SPINDLE_MIN_VALUE; // $35 Percent (extended set) - settings.spindle_pwm_max_value = DEFAULT_SPINDLE_MAX_VALUE; // $36 Percent (extended set) - - settings.homing_dir_mask = DEFAULT_HOMING_DIR_MASK; - settings.homing_feed_rate = DEFAULT_HOMING_FEED_RATE; - settings.homing_seek_rate = DEFAULT_HOMING_SEEK_RATE; - settings.homing_debounce_delay = DEFAULT_HOMING_DEBOUNCE_DELAY; - settings.homing_pulloff = DEFAULT_HOMING_PULLOFF; - settings.flags = 0; - if (DEFAULT_REPORT_INCHES) settings.flags |= BITFLAG_REPORT_INCHES; - if (DEFAULT_LASER_MODE) settings.flags |= BITFLAG_LASER_MODE; - if (DEFAULT_INVERT_ST_ENABLE) settings.flags |= BITFLAG_INVERT_ST_ENABLE; - if (DEFAULT_HARD_LIMIT_ENABLE) settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; - if (DEFAULT_HOMING_ENABLE) settings.flags |= BITFLAG_HOMING_ENABLE; - if (DEFAULT_SOFT_LIMIT_ENABLE) settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; - if (DEFAULT_INVERT_LIMIT_PINS) settings.flags |= BITFLAG_INVERT_LIMIT_PINS; - if (DEFAULT_INVERT_PROBE_PIN) settings.flags |= BITFLAG_INVERT_PROBE_PIN; - settings.steps_per_mm[X_AXIS] = DEFAULT_X_STEPS_PER_MM; - settings.steps_per_mm[Y_AXIS] = DEFAULT_Y_STEPS_PER_MM; - settings.steps_per_mm[Z_AXIS] = DEFAULT_Z_STEPS_PER_MM; - settings.max_rate[X_AXIS] = DEFAULT_X_MAX_RATE; - settings.max_rate[Y_AXIS] = DEFAULT_Y_MAX_RATE; - settings.max_rate[Z_AXIS] = DEFAULT_Z_MAX_RATE; - settings.acceleration[X_AXIS] = DEFAULT_X_ACCELERATION; - settings.acceleration[Y_AXIS] = DEFAULT_Y_ACCELERATION; - settings.acceleration[Z_AXIS] = DEFAULT_Z_ACCELERATION; - settings.max_travel[X_AXIS] = (-DEFAULT_X_MAX_TRAVEL); - settings.max_travel[Y_AXIS] = (-DEFAULT_Y_MAX_TRAVEL); - settings.max_travel[Z_AXIS] = (-DEFAULT_Z_MAX_TRAVEL); - settings.current[X_AXIS] = DEFAULT_X_CURRENT; - settings.current[Y_AXIS] = DEFAULT_Y_CURRENT; - settings.current[Z_AXIS] = DEFAULT_Z_CURRENT; - settings.hold_current[X_AXIS] = DEFAULT_X_HOLD_CURRENT; - settings.hold_current[Y_AXIS] = DEFAULT_Y_HOLD_CURRENT; - settings.hold_current[Z_AXIS] = DEFAULT_Z_HOLD_CURRENT; - settings.microsteps[X_AXIS] = DEFAULT_X_MICROSTEPS; - settings.microsteps[Y_AXIS] = DEFAULT_Y_MICROSTEPS; - settings.microsteps[Z_AXIS] = DEFAULT_Z_MICROSTEPS; - settings.stallguard[X_AXIS] = DEFAULT_X_STALLGUARD; - settings.stallguard[Y_AXIS] = DEFAULT_Y_STALLGUARD; - settings.stallguard[Z_AXIS] = DEFAULT_Z_STALLGUARD; -#if (N_AXIS > A_AXIS) - settings.steps_per_mm[A_AXIS] = DEFAULT_A_STEPS_PER_MM; - settings.max_rate[A_AXIS] = DEFAULT_A_MAX_RATE; - settings.acceleration[A_AXIS] = DEFAULT_A_ACCELERATION; - settings.max_travel[A_AXIS] = (-DEFAULT_A_MAX_TRAVEL); - settings.current[A_AXIS] = DEFAULT_A_CURRENT; - settings.hold_current[A_AXIS] = DEFAULT_A_HOLD_CURRENT; - settings.microsteps[A_AXIS] = DEFAULT_A_MICROSTEPS; - settings.stallguard[A_AXIS] = DEFAULT_Z_STALLGUARD; -#endif -#if (N_AXIS > B_AXIS) - settings.steps_per_mm[B_AXIS] = DEFAULT_B_STEPS_PER_MM; - settings.max_rate[B_AXIS] = DEFAULT_B_MAX_RATE; - settings.acceleration[B_AXIS] = DEFAULT_B_ACCELERATION; - settings.max_travel[B_AXIS] = (-DEFAULT_B_MAX_TRAVEL); - settings.current[B_AXIS] = DEFAULT_B_CURRENT; - settings.hold_current[B_AXIS] = DEFAULT_B_HOLD_CURRENT; - settings.microsteps[B_AXIS] = DEFAULT_B_MICROSTEPS; - settings.stallguard[B_AXIS] = DEFAULT_Z_STALLGUARD; -#endif -#if (N_AXIS > C_AXIS) - settings.steps_per_mm[C_AXIS] = DEFAULT_C_STEPS_PER_MM; - settings.max_rate[C_AXIS] = DEFAULT_C_MAX_RATE; - settings.acceleration[C_AXIS] = DEFAULT_C_ACCELERATION; - settings.max_travel[C_AXIS] = (-DEFAULT_C_MAX_TRAVEL); - settings.current[C_AXIS] = DEFAULT_C_CURRENT; - settings.hold_current[C_AXIS] = DEFAULT_C_HOLD_CURRENT; - settings.microsteps[C_AXIS] = DEFAULT_C_MICROSTEPS; - settings.stallguard[C_AXIS] = DEFAULT_Z_STALLGUARD; -#endif - // TODO figure out a clean way to add actual default values - for (uint8_t index = 0; index < USER_SETTING_COUNT; index++) { - settings.machine_int16[index] = 0; - settings.machine_float[index] = 0.0; - } - // User Integer values - settings.machine_int16[0] = DEFAULT_USER_INT_80; - settings.machine_int16[1] = DEFAULT_USER_INT_81; - settings.machine_int16[2] = DEFAULT_USER_INT_82; - settings.machine_int16[3] = DEFAULT_USER_INT_83; - settings.machine_int16[4] = DEFAULT_USER_INT_84; - // User Integer values - settings.machine_float[0] = DEFAULT_USER_FLOAT_90; - settings.machine_float[1] = DEFAULT_USER_FLOAT_91; - settings.machine_float[2] = DEFAULT_USER_FLOAT_92; - settings.machine_float[3] = DEFAULT_USER_FLOAT_93; - settings.machine_float[4] = DEFAULT_USER_FLOAT_94; - write_global_settings(); - } - if (restore_flag & SETTINGS_RESTORE_PARAMETERS) { - uint8_t idx; - float coord_data[N_AXIS]; - memset(&coord_data, 0, sizeof(coord_data)); - for (idx = 0; idx <= SETTING_INDEX_NCOORD; idx++) settings_write_coord_data(idx, coord_data); - } - if (restore_flag & SETTINGS_RESTORE_STARTUP_LINES) { -#if N_STARTUP_LINE > 0 - EEPROM.write(EEPROM_ADDR_STARTUP_BLOCK, 0); - EEPROM.write(EEPROM_ADDR_STARTUP_BLOCK + 1, 0); // Checksum - EEPROM.commit(); -#endif -#if N_STARTUP_LINE > 1 - EEPROM.write(EEPROM_ADDR_STARTUP_BLOCK + (LINE_BUFFER_SIZE + 1), 0); - EEPROM.write(EEPROM_ADDR_STARTUP_BLOCK + (LINE_BUFFER_SIZE + 2), 0); // Checksum - EEPROM.commit(); -#endif - } - if (restore_flag & SETTINGS_RESTORE_BUILD_INFO) { - EEPROM.write(EEPROM_ADDR_BUILD_INFO, 0); - EEPROM.write(EEPROM_ADDR_BUILD_INFO + 1, 0); // Checksum - EEPROM.commit(); - } -} - -// Reads Grbl global settings struct from EEPROM. -uint8_t read_global_settings() { - // Check version-byte of eeprom - uint8_t version = EEPROM.read(0); - if (version == SETTINGS_VERSION) { - // Read settings-record and check checksum - if (!(memcpy_from_eeprom_with_checksum((char*)&settings, EEPROM_ADDR_GLOBAL, sizeof(settings_t)))) - return (false); - } else - return (false); - return (true); -} - -// Method to store Grbl global settings struct and version number into EEPROM -// NOTE: This function can only be called in IDLE state. -void write_global_settings() { - EEPROM.write(0, SETTINGS_VERSION); - memcpy_to_eeprom_with_checksum(EEPROM_ADDR_GLOBAL, (char*)&settings, sizeof(settings_t)); -} // Read selected coordinate data from EEPROM. Updates pointed coord_data value. uint8_t settings_read_coord_data(uint8_t coord_select, float* coord_data) { @@ -227,7 +48,7 @@ void settings_write_coord_data(uint8_t coord_select, float* coord_data) { // Method to store build info into EEPROM // NOTE: This function can only be called in IDLE state. -void settings_store_build_info(char* line) { +void settings_store_build_info(const char* line) { // Build info can only be stored when state is IDLE. memcpy_to_eeprom_with_checksum(EEPROM_ADDR_BUILD_INFO, (char*)line, LINE_BUFFER_SIZE); } @@ -243,175 +64,15 @@ uint8_t settings_read_build_info(char* line) { return (true); } - - -// Reads startup line from EEPROM. Updated pointed line string data. -uint8_t settings_read_startup_line(uint8_t n, char* line) { - uint32_t addr = n * (LINE_BUFFER_SIZE + 1) + EEPROM_ADDR_STARTUP_BLOCK; - if (!(memcpy_from_eeprom_with_checksum((char*)line, addr, LINE_BUFFER_SIZE))) { - // Reset line with default value - line[0] = 0; // Empty line - settings_store_startup_line(n, line); - return (false); - } - return (true); -} - -// A helper method to set settings from command line -uint8_t settings_store_global_setting(uint8_t parameter, float value) { - if (value < 0.0) return (STATUS_NEGATIVE_VALUE); - uint8_t int_value = trunc(value); // integer version - if (parameter >= AXIS_SETTINGS_START_VAL) { - // Store axis configuration. Axis numbering sequence set by AXIS_SETTING defines. - // NOTE: Ensure the setting index corresponds to the report.c settings printout. - parameter -= AXIS_SETTINGS_START_VAL; - uint8_t set_idx = 0; - while (set_idx < AXIS_N_SETTINGS) { - if (parameter < N_AXIS) { - // Valid axis setting found. - switch (set_idx) { - case 0: -#ifdef MAX_STEP_RATE_HZ - if (value * settings.max_rate[parameter] > (MAX_STEP_RATE_HZ * 60.0)) return (STATUS_MAX_STEP_RATE_EXCEEDED); -#endif - settings.steps_per_mm[parameter] = value; - break; - case 1: -#ifdef MAX_STEP_RATE_HZ - if (value * settings.steps_per_mm[parameter] > (MAX_STEP_RATE_HZ * 60.0)) return (STATUS_MAX_STEP_RATE_EXCEEDED); -#endif - settings.max_rate[parameter] = value; - break; - case 2: settings.acceleration[parameter] = value * 60 * 60; break; // Convert to mm/min^2 for grbl internal use. - case 3: settings.max_travel[parameter] = -value; break; // Store as negative for grbl internal use. - case 4: // run current - settings.current[parameter] = value; - settings_spi_driver_init(); - break; - case 5: // hold current - settings.hold_current[parameter] = value; - settings_spi_driver_init(); - break; - case 6: // microstepping - settings.microsteps[parameter] = int_value; - settings_spi_driver_init(); - break; - case 7: // stallguard - settings.stallguard[parameter] = int_value; - settings_spi_driver_init(); - break; - } - break; // Exit while-loop after setting has been configured and proceed to the EEPROM write call. - } else { - set_idx++; - // If axis index greater than N_AXIS or setting index greater than number of axis settings, error out. - if ((parameter < AXIS_SETTINGS_INCREMENT) || (set_idx == AXIS_N_SETTINGS)) return (STATUS_INVALID_STATEMENT); - parameter -= AXIS_SETTINGS_INCREMENT; - } - } - } else { - // Store non-axis Grbl settings - switch (parameter) { - case 0: - if (int_value < 3) return (STATUS_SETTING_STEP_PULSE_MIN); - settings.pulse_microseconds = int_value; break; - case 1: settings.stepper_idle_lock_time = int_value; break; - case 2: - settings.step_invert_mask = int_value; - st_generate_step_dir_invert_masks(); // Regenerate step and direction port invert masks. - break; - case 3: - settings.dir_invert_mask = int_value; - st_generate_step_dir_invert_masks(); // Regenerate step and direction port invert masks. - break; - case 4: // Reset to ensure change. Immediate re-init may cause problems. - if (int_value) settings.flags |= BITFLAG_INVERT_ST_ENABLE; - else settings.flags &= ~BITFLAG_INVERT_ST_ENABLE; - break; - case 5: // Reset to ensure change. Immediate re-init may cause problems. - if (int_value) settings.flags |= BITFLAG_INVERT_LIMIT_PINS; - else settings.flags &= ~BITFLAG_INVERT_LIMIT_PINS; - break; - case 6: // Reset to ensure change. Immediate re-init may cause problems. - if (int_value) settings.flags |= BITFLAG_INVERT_PROBE_PIN; - else settings.flags &= ~BITFLAG_INVERT_PROBE_PIN; - probe_configure_invert_mask(false); - break; - case 10: settings.status_report_mask = int_value; break; - case 11: settings.junction_deviation = value; break; - case 12: settings.arc_tolerance = value; break; - case 13: - if (int_value) settings.flags |= BITFLAG_REPORT_INCHES; - else settings.flags &= ~BITFLAG_REPORT_INCHES; - system_flag_wco_change(); // Make sure WCO is immediately updated. - break; - case 20: - if (int_value) { - if (bit_isfalse(settings.flags, BITFLAG_HOMING_ENABLE)) return (STATUS_SOFT_LIMIT_ERROR); - settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; - } else settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; - break; - case 21: - if (int_value) settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; - else settings.flags &= ~BITFLAG_HARD_LIMIT_ENABLE; - limits_init(); // Re-init to immediately change. NOTE: Nice to have but could be problematic later. - break; - case 22: - if (int_value) settings.flags |= BITFLAG_HOMING_ENABLE; - else { - settings.flags &= ~BITFLAG_HOMING_ENABLE; - settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; // Force disable soft-limits. - } - break; - case 23: settings.homing_dir_mask = int_value; break; - case 24: settings.homing_feed_rate = value; break; - case 25: settings.homing_seek_rate = value; break; - case 26: settings.homing_debounce_delay = int_value; break; - case 27: settings.homing_pulloff = value; break; - case 30: settings.rpm_max = std::max(value, 1.0f); spindle->init(); break; // Re-initialize spindle rpm calibration (min of 1) - case 31: settings.rpm_min = value; spindle->init(); break; // Re-initialize spindle rpm calibration - case 32: - if (int_value) - settings.flags |= BITFLAG_LASER_MODE; - else - settings.flags &= ~BITFLAG_LASER_MODE; - spindle->init(); // update the spindle class - break; - case 33: settings.spindle_pwm_freq = value; spindle_select(SPINDLE_TYPE); break; // Re-initialize spindle pwm calibration - case 34: settings.spindle_pwm_off_value = value; spindle_select(SPINDLE_TYPE); break; // Re-initialize spindle pwm calibration - case 35: settings.spindle_pwm_min_value = value; spindle_select(SPINDLE_TYPE); break; // Re-initialize spindle pwm calibration - case 36: settings.spindle_pwm_max_value = value; spindle_select(SPINDLE_TYPE); break; // Re-initialize spindle pwm calibration - case 80: - case 81: - case 82: - case 83: - case 84: - settings.machine_int16[parameter - 80] = (uint16_t)value; - break; - case 90: - case 91: - case 92: - case 93: - case 94: - settings.machine_float[parameter - 90] = value; - break; - default: - return (STATUS_INVALID_STATEMENT); - } - } - write_global_settings(); - return (STATUS_OK); -} - // Returns step pin mask according to Grbl internal axis indexing. uint8_t get_step_pin_mask(uint8_t axis_idx) { // todo clean this up further up stream - return (1 << axis_idx); + return bit(axis_idx); } // Returns direction pin mask according to Grbl internal axis indexing. uint8_t get_direction_pin_mask(uint8_t axis_idx) { - return (1 << axis_idx); + return bit(axis_idx); } // this allows a conditional re-init of the trinamic settings @@ -421,4 +82,4 @@ void settings_spi_driver_init() { #else grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "No SPI drivers setup"); #endif -} \ No newline at end of file +} diff --git a/Grbl_Esp32/settings.h b/Grbl_Esp32/settings.h index a625438a..3b3cb692 100644 --- a/Grbl_Esp32/settings.h +++ b/Grbl_Esp32/settings.h @@ -28,21 +28,7 @@ #include "grbl.h" -// Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl -// when firmware is upgraded. Always stored in byte 0 of eeprom -#define SETTINGS_VERSION 12 // NOTE: Check settings_reset() when moving to next version. - -// Define bit flag masks for the boolean settings in settings.flag. -#define BITFLAG_REPORT_INCHES bit(0) -#define BITFLAG_LASER_MODE bit(1) -#define BITFLAG_INVERT_ST_ENABLE bit(2) -#define BITFLAG_HARD_LIMIT_ENABLE bit(3) -#define BITFLAG_HOMING_ENABLE bit(4) -#define BITFLAG_SOFT_LIMIT_ENABLE bit(5) -#define BITFLAG_INVERT_LIMIT_PINS bit(6) -#define BITFLAG_INVERT_PROBE_PIN bit(7) - -// Define status reporting boolean enable bit flags in settings.status_report_mask +// Define status reporting boolean enable bit flags in status_report_mask #define BITFLAG_RT_STATUS_POSITION_TYPE bit(0) #define BITFLAG_RT_STATUS_BUFFER_STATE bit(1) @@ -61,9 +47,7 @@ // the startup script. The lower half contains the global settings and space for future // developments. #define EEPROM_SIZE 1024U -#define EEPROM_ADDR_GLOBAL 1U #define EEPROM_ADDR_PARAMETERS 512U -#define EEPROM_ADDR_STARTUP_BLOCK 768U #define EEPROM_ADDR_BUILD_INFO 942U // Define EEPROM address indexing for coordinate parameters @@ -74,70 +58,15 @@ #define SETTING_INDEX_G30 N_COORDINATE_SYSTEM+1 // Home position 2 // #define SETTING_INDEX_G92 N_COORDINATE_SYSTEM+2 // Coordinate offset (G92.2,G92.3 not supported) -// Define Grbl axis settings numbering scheme. Starts at START_VAL, every INCREMENT, over N_SETTINGS. -#define AXIS_N_SETTINGS 8 // includes extended settings - -#define AXIS_SETTINGS_START_VAL 100 // NOTE: Reserving settings values >= 100 for axis settings. Up to 255. -#define AXIS_SETTINGS_INCREMENT 10 // Must be greater than the number of axis settings - #define USER_SETTING_COUNT 5 // for user to define for their machine -// Global persistent settings (Stored from byte EEPROM_ADDR_GLOBAL onwards) -typedef struct { - // Axis settings - float steps_per_mm[N_AXIS]; - float max_rate[N_AXIS]; - float acceleration[N_AXIS]; - float max_travel[N_AXIS]; - float current[N_AXIS]; // $140... run current (extended set) - float hold_current[N_AXIS]; // $150 percent of run current (extended set) - uint16_t microsteps[N_AXIS]; // $160... (extended set) - uint8_t stallguard[N_AXIS]; // $170... (extended set) - - // Remaining Grbl settings - uint8_t pulse_microseconds; - uint8_t step_invert_mask; - uint8_t dir_invert_mask; - uint8_t stepper_idle_lock_time; // If max value 255, steppers do not disable. - uint8_t status_report_mask; // Mask to indicate desired report data. - float junction_deviation; - float arc_tolerance; - - float spindle_pwm_freq; // $33 Hz (extended set) - float spindle_pwm_off_value; // $34 Percent (extended set) - float spindle_pwm_min_value; // $35 Percent (extended set) - float spindle_pwm_max_value; // $36 Percent (extended set) - - float rpm_max; - float rpm_min; - - uint8_t flags; // Contains default boolean settings - - uint8_t homing_dir_mask; - float homing_feed_rate; - float homing_seek_rate; - uint16_t homing_debounce_delay; - float homing_pulloff; - - int16_t machine_int16[USER_SETTING_COUNT]; // settings starting at 80 to be defined by the user - float machine_float[USER_SETTING_COUNT]; // settings starting at 80 to be defined by the user - -} settings_t; -extern settings_t settings; - // Initialize the configuration subsystem (load settings from EEPROM) void settings_init(); void settings_restore(uint8_t restore_flag); void write_global_settings(); -uint8_t read_global_settings(); - -uint8_t settings_read_startup_line(uint8_t n, char* line); -void settings_store_startup_line(uint8_t n, char* line); uint8_t settings_read_build_info(char* line); -void settings_store_build_info(char* line); - -uint8_t settings_store_global_setting(uint8_t parameter, float value); +void settings_store_build_info(const char* line); // Writes selected coordinate data to EEPROM void settings_write_coord_data(uint8_t coord_select, float* coord_data); diff --git a/Grbl_Esp32/stepper.cpp b/Grbl_Esp32/stepper.cpp index 869f5f18..36756d93 100644 --- a/Grbl_Esp32/stepper.cpp +++ b/Grbl_Esp32/stepper.cpp @@ -147,21 +147,7 @@ typedef struct { } st_prep_t; static st_prep_t prep; -// RMT channel numbers. These are assigned dynamically as needed via the CPU MAP -// Only 8 are available (0-7) -// They are Initialized with an invalid number to prevent unitended consequences -uint8_t X_rmt_chan_num = 255; -uint8_t X2_rmt_chan_num = 255; // Ganged axes have the "2" -uint8_t Y_rmt_chan_num = 255; -uint8_t Y2_rmt_chan_num = 255; -uint8_t Z_rmt_chan_num = 255; -uint8_t Z2_rmt_chan_num = 255; -uint8_t A_rmt_chan_num = 255; -uint8_t A2_rmt_chan_num = 255; -uint8_t B_rmt_chan_num = 255; -uint8_t B2_rmt_chan_num = 255; -uint8_t C_rmt_chan_num = 255; -uint8_t C2_rmt_chan_num = 255; + /* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. Grbl employs the venerable Bresenham line algorithm to manage and exactly synchronize multi-axis moves. @@ -212,7 +198,7 @@ uint8_t C2_rmt_chan_num = 255; Direction pin is set An optional (via STEP_PULSE_DELAY in config.h) is put after this The step pin is started - A pulse length is determine (via option $0 ... settings.pulse_microseconds) + A pulse length is determine (via option $0 ... pulse_microseconds) The pulse is ended Direction will remain the same until another step occurs with a change in direction. @@ -221,29 +207,47 @@ uint8_t C2_rmt_chan_num = 255; #ifdef USE_RMT_STEPS inline IRAM_ATTR static void stepperRMT_Outputs(); #endif + +static void stepper_pulse_func(); + // TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller // int8 variables and update position counters only when a segment completes. This can get complicated // with probing and homing cycles that require true real-time positions. void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a step ======================================================================================= -#ifndef USE_RMT_STEPS - uint64_t step_pulse_off_time; -#endif //const int timer_idx = (int)para; // get the timer index TIMERG0.int_clr_timers.t0 = 1; if (busy) { return; // The busy-flag is used to avoid reentering this interrupt } - set_direction_pins_on(st.dir_outbits); + busy = true; + + stepper_pulse_func(); + + TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN; + busy = false; +} + +/** + * This phase of the ISR should ONLY create the pulses for the steppers. + * This prevents jitter caused by the interval between the start of the + * interrupt and the start of the pulses. DON'T add any logic ahead of the + * call to this method that might cause variation in the timing. The aim + * is to keep pulse timing as regular as possible. + */ +static void stepper_pulse_func() { + motors_set_direction_pins(st.dir_outbits); #ifdef USE_RMT_STEPS stepperRMT_Outputs(); #else set_stepper_pins_on(st.step_outbits); - step_pulse_off_time = esp_timer_get_time() + (settings.pulse_microseconds); // determine when to turn off pulse +#ifndef USE_I2S_OUT_STREAM + uint64_t step_pulse_start_time = esp_timer_get_time(); #endif -#ifdef USE_UNIPOLAR - unipolar_step(st.step_outbits, st.dir_outbits); #endif - busy = true; + + // some motor objects, like unipolar, handle steps themselves + motors_step(st.step_outbits, st.dir_outbits); + // If there is no step segment, attempt to pop one from the stepper buffer if (st.exec_segment == NULL) { // Anything in the buffer? If so, load and initialize next step segment. @@ -262,7 +266,7 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st st.counter_x = st.counter_y = st.counter_z = (st.exec_block->step_event_count >> 1); // TODO ABC } - st.dir_outbits = st.exec_block->direction_bits ^ settings.dir_invert_mask; + st.dir_outbits = st.exec_block->direction_bits ^ dir_invert_mask->get(); #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING // With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level. st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; @@ -285,9 +289,8 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st st_go_idle(); if (!(sys.state & STATE_JOG)) { // added to prevent ... jog after probing crash // Ensure pwm is set properly upon completion of rate-controlled motion. - if (st.exec_block->is_pwm_rate_adjusted) { + if (st.exec_block != NULL && st.exec_block->is_pwm_rate_adjusted) spindle->set_rpm(0); - } } system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end @@ -306,9 +309,9 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st st.counter_x += st.exec_block->steps[X_AXIS]; #endif if (st.counter_x > st.exec_block->step_event_count) { - st.step_outbits |= (1 << X_STEP_BIT); + st.step_outbits |= bit(X_AXIS); st.counter_x -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & (1 << X_DIRECTION_BIT)) + if (st.exec_block->direction_bits & bit(X_AXIS)) sys_position[X_AXIS]--; else sys_position[X_AXIS]++; @@ -319,9 +322,9 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st st.counter_y += st.exec_block->steps[Y_AXIS]; #endif if (st.counter_y > st.exec_block->step_event_count) { - st.step_outbits |= (1 << Y_STEP_BIT); + st.step_outbits |= bit(Y_AXIS); st.counter_y -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & (1 << Y_DIRECTION_BIT)) + if (st.exec_block->direction_bits & bit(Y_AXIS)) sys_position[Y_AXIS]--; else sys_position[Y_AXIS]++; @@ -332,9 +335,9 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st st.counter_z += st.exec_block->steps[Z_AXIS]; #endif if (st.counter_z > st.exec_block->step_event_count) { - st.step_outbits |= (1 << Z_STEP_BIT); + st.step_outbits |= bit(Z_AXIS); st.counter_z -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & (1 << Z_DIRECTION_BIT)) + if (st.exec_block->direction_bits & bit(Z_AXIS)) sys_position[Z_AXIS]--; else sys_position[Z_AXIS]++; @@ -346,9 +349,9 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st st.counter_a += st.exec_block->steps[A_AXIS]; #endif if (st.counter_a > st.exec_block->step_event_count) { - st.step_outbits |= (1 << A_STEP_BIT); + st.step_outbits |= bit(A_AXIS); st.counter_a -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & (1 << A_DIRECTION_BIT)) sys_position[A_AXIS]--; + if (st.exec_block->direction_bits & bit(A_AXIS)) sys_position[A_AXIS]--; else sys_position[A_AXIS]++; } #endif @@ -359,9 +362,9 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st st.counter_b += st.exec_block->steps[B_AXIS]; #endif if (st.counter_b > st.exec_block->step_event_count) { - st.step_outbits |= (1 << B_STEP_BIT); + st.step_outbits |= bit(B_AXIS); st.counter_b -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & (1 << B_DIRECTION_BIT)) sys_position[B_AXIS]--; + if (st.exec_block->direction_bits & bit(B_AXIS)) sys_position[B_AXIS]--; else sys_position[B_AXIS]++; } #endif @@ -372,9 +375,9 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st st.counter_c += st.exec_block->steps[C_AXIS]; #endif if (st.counter_c > st.exec_block->step_event_count) { - st.step_outbits |= (1 << C_STEP_BIT); + st.step_outbits |= bit(C_AXIS); st.counter_c -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & (1 << C_DIRECTION_BIT)) sys_position[C_AXIS]--; + if (st.exec_block->direction_bits & bit(C_AXIS)) sys_position[C_AXIS]--; else sys_position[C_AXIS]++; } #endif @@ -388,112 +391,44 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st if (++segment_buffer_tail == SEGMENT_BUFFER_SIZE) segment_buffer_tail = 0; } + #ifndef USE_RMT_STEPS +#ifdef USE_I2S_OUT_STREAM + // + // 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 +#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_off_time) { + while (esp_timer_get_time() - step_pulse_start_time < pulse_microseconds->get()) { NOP(); // spin here until time to turn off step } set_stepper_pins_on(0); // turn all off #endif - TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN; - busy = false; +#endif + return; } - void stepper_init() { - // make the stepper disable pin an output -#ifdef STEPPERS_DISABLE_PIN - pinMode(STEPPERS_DISABLE_PIN, OUTPUT); - set_stepper_disable(true); -#endif -#ifdef USE_UNIPOLAR - unipolar_init(); -#endif -#ifdef USE_TRINAMIC - Trinamic_Init(); -#endif + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Axis count %d", N_AXIS); + // make the step pins outputs #ifdef USE_RMT_STEPS grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "RMT Steps"); - initRMT(); +#elif defined(USE_I2S_OUT_STREAM) + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "I2S Steps"); #else grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Timed Steps"); - // make the step pins outputs -#ifdef X_STEP_PIN - pinMode(X_STEP_PIN, OUTPUT); #endif -#ifdef X2_STEP_PIN // ganged motor - pinMode(X2_STEP_PIN, OUTPUT); -#endif -#ifdef Y_STEP_PIN - pinMode(Y_STEP_PIN, OUTPUT); -#endif -#ifdef Y2_STEP_PIN - pinMode(Y2_STEP_PIN, OUTPUT); -#endif -#ifdef Z_STEP_PIN - pinMode(Z_STEP_PIN, OUTPUT); -#endif -#ifdef Z2_STEP_PIN - pinMode(Z2_STEP_PIN, OUTPUT); -#endif -#ifdef A_STEP_PIN - pinMode(A_STEP_PIN, OUTPUT); -#endif -#ifdef B_STEP_PIN - pinMode(B_STEP_PIN, OUTPUT); -#endif -#ifdef C_STEP_PIN - pinMode(C_STEP_PIN, OUTPUT); -#endif -#endif - // make the direction pins outputs -#ifdef X_DIRECTION_PIN - pinMode(X_DIRECTION_PIN, OUTPUT); -#endif -#ifdef X2_DIRECTION_PIN - pinMode(X2_DIRECTION_PIN, OUTPUT); -#endif -#ifdef Y_DIRECTION_PIN - pinMode(Y_DIRECTION_PIN, OUTPUT); -#endif -#ifdef Y2_DIRECTION_PIN - pinMode(Y2_DIRECTION_PIN, OUTPUT); -#endif -#ifdef Z_DIRECTION_PIN - pinMode(Z_DIRECTION_PIN, OUTPUT); -#endif -#ifdef Z2_DIRECTION_PIN - pinMode(Z2_DIRECTION_PIN, OUTPUT); -#endif -#ifdef A_DIRECTION_PIN - pinMode(A_DIRECTION_PIN, OUTPUT); -#endif -#ifdef A2_DIRECTION_PIN - pinMode(A2_DIRECTION_PIN, OUTPUT); -#endif -#ifdef B_DIRECTION_PIN - pinMode(B_DIRECTION_PIN, OUTPUT); -#endif -#ifdef B2_DIRECTION_PIN - pinMode(B2_DIRECTION_PIN, OUTPUT); -#endif -#ifdef C_DIRECTION_PIN - pinMode(C_DIRECTION_PIN, OUTPUT); -#endif -#ifdef C2_DIRECTION_PIN - pinMode(C2_DIRECTION_PIN, OUTPUT); -#endif - // setup stepper timer interrupt - /* - stepperDriverTimer = timerBegin( 0, // timer number - F_TIMERS / F_STEPPER_TIMER, // prescaler - true // auto reload - ); - // attach the interrupt - timerAttachInterrupt(stepperDriverTimer, &onStepperDriverTimer, true); - */ + +#ifdef USE_I2S_OUT_STREAM + // I2S stepper do not use timer interrupt but callback + i2s_out_set_pulse_callback(stepper_pulse_func); +#else timer_config_t config; config.divider = F_TIMERS / F_STEPPER_TIMER; config.counter_dir = TIMER_COUNT_UP; @@ -505,138 +440,19 @@ void stepper_init() { timer_set_counter_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, 0x00000000ULL); timer_enable_intr(STEP_TIMER_GROUP, STEP_TIMER_INDEX); timer_isr_register(STEP_TIMER_GROUP, STEP_TIMER_INDEX, onStepperDriverTimer, NULL, 0, NULL); +#endif +#ifdef USE_TRINAMIC + Trinamic_Init(); +#endif } -#ifdef USE_RMT_STEPS -void initRMT() { - rmt_item32_t rmtItem[2]; - rmt_config_t rmtConfig; - rmtConfig.rmt_mode = RMT_MODE_TX; - rmtConfig.clk_div = 20; - rmtConfig.mem_block_num = 2; - rmtConfig.tx_config.loop_en = false; - rmtConfig.tx_config.carrier_en = false; - rmtConfig.tx_config.carrier_freq_hz = 0; - rmtConfig.tx_config.carrier_duty_percent = 50; - rmtConfig.tx_config.carrier_level = RMT_CARRIER_LEVEL_LOW; - rmtConfig.tx_config.idle_output_en = true; -#ifdef STEP_PULSE_DELAY - rmtItem[0].duration0 = STEP_PULSE_DELAY * 4; -#else - rmtItem[0].duration0 = 1; -#endif - rmtItem[0].duration1 = 4 * settings.pulse_microseconds; - rmtItem[1].duration0 = 0; - rmtItem[1].duration1 = 0; -#ifdef X_STEP_PIN - X_rmt_chan_num = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)X_rmt_chan_num, RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)X_rmt_chan_num; - rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, X_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = X_STEP_PIN; - rmtItem[0].level0 = rmtConfig.tx_config.idle_level; - rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; - rmt_config(&rmtConfig); - rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); -#endif -#ifdef X2_STEP_PIN - X2_rmt_chan_num = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)X2_rmt_chan_num, RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)X2_rmt_chan_num; - rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, X_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = X2_STEP_PIN; - rmtItem[0].level0 = rmtConfig.tx_config.idle_level; - rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; - rmt_config(&rmtConfig); - rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); -#endif -#ifdef Y_STEP_PIN - Y_rmt_chan_num = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)Y_rmt_chan_num, RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)Y_rmt_chan_num; - rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, Y_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = Y_STEP_PIN; - rmtItem[0].level0 = rmtConfig.tx_config.idle_level; - rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; - rmt_config(&rmtConfig); - rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); -#endif -#ifdef Y2_STEP_PIN - Y2_rmt_chan_num = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)Y2_rmt_chan_num, RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)Y2_rmt_chan_num; - rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, Y_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = Y2_STEP_PIN; - rmtItem[0].level0 = rmtConfig.tx_config.idle_level; - rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; - rmt_config(&rmtConfig); - rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); -#endif -#ifdef Z_STEP_PIN - Z_rmt_chan_num = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)Z_rmt_chan_num, RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)Z_rmt_chan_num; - rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, Z_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = Z_STEP_PIN; - rmtItem[0].level0 = rmtConfig.tx_config.idle_level; - rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; - rmt_config(&rmtConfig); - rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); -#endif -#ifdef Z2_STEP_PIN - Z2_rmt_chan_num = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)Z2_rmt_chan_num, RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)Z2_rmt_chan_num; - rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, Z_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = Z2_STEP_PIN; - rmtItem[0].level0 = rmtConfig.tx_config.idle_level; - rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; - rmt_config(&rmtConfig); - rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); -#endif -#ifdef A_STEP_PIN - A_rmt_chan_num = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)A_rmt_chan_num, RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)A_rmt_chan_num; - rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, A_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = A_STEP_PIN; // TODO - rmtItem[0].level0 = rmtConfig.tx_config.idle_level; - rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; - rmt_config(&rmtConfig); - rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); -#endif -#ifdef B_STEP_PIN - B_rmt_chan_num = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)B_rmt_chan_num, RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)B_rmt_chan_num; - rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, B_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = B_STEP_PIN; // TODO - rmtItem[0].level0 = rmtConfig.tx_config.idle_level; - rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; - rmt_config(&rmtConfig); - rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); -#endif -#ifdef C_STEP_PIN - C_rmt_chan_num = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)C_rmt_chan_num, RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)C_rmt_chan_num; - rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, C_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = C_STEP_PIN; // TODO - rmtItem[0].level0 = rmtConfig.tx_config.idle_level; - rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; - rmt_config(&rmtConfig); - rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); -#endif -} -#endif + // enabled. Startup init and limits call this function but shouldn't start the cycle. void st_wake_up() { -#ifdef ESP_DEBUG - //Serial.println("st_wake_up()"); -#endif + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "st_wake_up"); // Enable stepper drivers. - set_stepper_disable(false); + 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; @@ -645,7 +461,7 @@ void st_wake_up() { // Step pulse delay handling is not require with ESP32...the RMT function does it. #else // Normal operation // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement. - st.step_pulse_time = -(((settings.pulse_microseconds - 2) * TICKS_PER_MICROSECOND) >> 3); + st.step_pulse_time = -(((pulse_microseconds->get() - 2) * TICKS_PER_MICROSECOND) >> 3); #endif // Enable Stepper Driver Interrupt Stepper_Timer_Start(); @@ -657,6 +473,9 @@ void st_reset() { //Serial.println("st_reset()"); #endif // Initialize stepper driver idle state. +#ifdef USE_I2S_OUT_STREAM + i2s_out_reset(); +#endif st_go_idle(); // Initialize stepper algorithm variables. memset(&prep, 0, sizeof(st_prep_t)); @@ -673,176 +492,185 @@ void st_reset() { } - - - -void set_direction_pins_on(uint8_t onMask) { - // inverts are applied in step generation -#ifdef X_DIRECTION_PIN - digitalWrite(X_DIRECTION_PIN, (onMask & (1 << X_AXIS))); -#endif -#ifdef X2_DIRECTION_PIN // optional ganged axis - digitalWrite(X2_DIRECTION_PIN, (onMask & (1 << X_AXIS))); -#endif -#ifdef Y_DIRECTION_PIN - digitalWrite(Y_DIRECTION_PIN, (onMask & (1 << Y_AXIS))); -#endif -#ifdef Y2_DIRECTION_PIN // optional ganged axis - digitalWrite(Y2_DIRECTION_PIN, (onMask & (1 << Y_AXIS))); -#endif -#ifdef Z_DIRECTION_PIN - digitalWrite(Z_DIRECTION_PIN, (onMask & (1 << Z_AXIS))); -#endif -#ifdef Z2_DIRECTION_PIN // optional ganged axis - digitalWrite(Z2_DIRECTION_PIN, (onMask & (1 << Z_AXIS))); -#endif -#ifdef A_DIRECTION_PIN - digitalWrite(A_DIRECTION_PIN, (onMask & (1 << A_AXIS))); -#endif -#ifdef A2_DIRECTION_PIN // optional ganged axis - digitalWrite(A2_DIRECTION_PIN, (onMask & (1 << A_AXIS))); -#endif -#ifdef B_DIRECTION_PIN - digitalWrite(B_DIRECTION_PIN, (onMask & (1 << B_AXIS))); -#endif -#ifdef B2_DIRECTION_PIN // optional ganged axis - digitalWrite(B2_DIRECTION_PIN, (onMask & (1 << B_AXIS))); -#endif -#ifdef C_DIRECTION_PIN - digitalWrite(C_DIRECTION_PIN, (onMask & (1 << C_AXIS))); -#endif -#ifdef C2_DIRECTION_PIN // optional ganged axis - digitalWrite(C2_DIRECTION_PIN, (onMask & (1 << C_AXIS))); -#endif -} - -#ifndef USE_GANGED_AXES -// basic one motor per axis void set_stepper_pins_on(uint8_t onMask) { - onMask ^= settings.step_invert_mask; // invert pins as required by invert mask -#ifdef X_STEP_PIN - digitalWrite(X_STEP_PIN, (onMask & (1 << X_AXIS))); -#endif -#ifdef Y_STEP_PIN - digitalWrite(Y_STEP_PIN, (onMask & (1 << Y_AXIS))); -#endif -#ifdef Z_STEP_PIN - digitalWrite(Z_STEP_PIN, (onMask & (1 << Z_AXIS))); -#endif -#ifdef A_STEP_PIN - digitalWrite(A_STEP_PIN, (onMask & (1 << A_AXIS))); -#endif -} -#else // we use ganged axes -void set_stepper_pins_on(uint8_t onMask) { - onMask ^= settings.step_invert_mask; // invert pins as required by invert mask + onMask ^= step_invert_mask->get(); // invert pins as required by invert mask #ifdef X_STEP_PIN #ifndef X2_STEP_PIN // if not a ganged axis - digitalWrite(X_STEP_PIN, (onMask & (1 << X_AXIS))); + digitalWrite(X_STEP_PIN, (onMask & bit(X_AXIS))); #else // is a ganged axis if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) - digitalWrite(X_STEP_PIN, (onMask & (1 << X_AXIS))); + digitalWrite(X_STEP_PIN, (onMask & bit(X_AXIS))); if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) - digitalWrite(X2_STEP_PIN, (onMask & (1 << X_AXIS))); + digitalWrite(X2_STEP_PIN, (onMask & bit(X_AXIS))); #endif #endif #ifdef Y_STEP_PIN #ifndef Y2_STEP_PIN // if not a ganged axis - digitalWrite(Y_STEP_PIN, (onMask & (1 << Y_AXIS))); + digitalWrite(Y_STEP_PIN, (onMask & bit(Y_AXIS))); #else // is a ganged axis if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) - digitalWrite(Y_STEP_PIN, (onMask & (1 << Y_AXIS))); + digitalWrite(Y_STEP_PIN, (onMask & bit(Y_AXIS))); if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) - digitalWrite(Y2_STEP_PIN, (onMask & (1 << Y_AXIS))); + digitalWrite(Y2_STEP_PIN, (onMask & bit(Y_AXIS))); #endif #endif + #ifdef Z_STEP_PIN #ifndef Z2_STEP_PIN // if not a ganged axis - digitalWrite(Z_STEP_PIN, (onMask & (1 << Z_AXIS))); + digitalWrite(Z_STEP_PIN, (onMask & bit(Z_AXIS))); #else // is a ganged axis if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) - digitalWrite(Z_STEP_PIN, (onMask & (1 << Z_AXIS))); + digitalWrite(Z_STEP_PIN, (onMask & bit(Z_AXIS))); if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) - digitalWrite(Z2_STEP_PIN, (onMask & (1 << Z_AXIS))); + digitalWrite(Z2_STEP_PIN, (onMask & bit(Z_AXIS))); +#endif +#endif + +#ifdef A_STEP_PIN +#ifndef A2_STEP_PIN // if not a ganged axis + digitalWrite(A_STEP_PIN, (onMask & bit(A_AXIS))); +#else // is a ganged axis + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) + digitalWrite(A_STEP_PIN, (onMask & bit(A_AXIS))); + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) + digitalWrite(A2_STEP_PIN, (onMask & bit(A_AXIS))); +#endif +#endif + +#ifdef B_STEP_PIN +#ifndef B2_STEP_PIN // if not a ganged axis + digitalWrite(B_STEP_PIN, (onMask & bit(B_AXIS))); +#else // is a ganged axis + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) + digitalWrite(B_STEP_PIN, (onMask & bit(B_AXIS))); + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) + digitalWrite(B2_STEP_PIN, (onMask & bit(B_AXIS))); +#endif +#endif + +#ifdef C_STEP_PIN +#ifndef C2_STEP_PIN // if not a ganged axis + digitalWrite(C_STEP_PIN, (onMask & bit(C_AXIS))); +#else // is a ganged axis + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) + digitalWrite(C_STEP_PIN, (onMask & bit(C_AXIS))); + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) + digitalWrite(C2_STEP_PIN, (onMask & bit(C_AXIS))); #endif #endif } -#endif +//#endif #ifdef USE_RMT_STEPS -// Set stepper pulse output pins inline IRAM_ATTR static void stepperRMT_Outputs() { #ifdef X_STEP_PIN - if (st.step_outbits & (1 << X_AXIS)) { + if (st.step_outbits & bit(X_AXIS)) { #ifndef X2_STEP_PIN // if not a ganged axis - RMT.conf_ch[X_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[X_rmt_chan_num].conf1.tx_start = 1; + RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; #else // it is a ganged axis if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) { - RMT.conf_ch[X_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[X_rmt_chan_num].conf1.tx_start = 1; + RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; } if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) { - RMT.conf_ch[X2_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[X2_rmt_chan_num].conf1.tx_start = 1; + RMT.conf_ch[rmt_chan_num[X_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[X_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; } #endif } #endif #ifdef Y_STEP_PIN - if (st.step_outbits & (1 << Y_AXIS)) { + if (st.step_outbits & bit(Y_AXIS)) { #ifndef Y2_STEP_PIN // if not a ganged axis - RMT.conf_ch[Y_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[Y_rmt_chan_num].conf1.tx_start = 1; + RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; #else // it is a ganged axis if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) { - RMT.conf_ch[Y_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[Y_rmt_chan_num].conf1.tx_start = 1; + RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; } if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) { - RMT.conf_ch[Y2_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[Y2_rmt_chan_num].conf1.tx_start = 1; + RMT.conf_ch[rmt_chan_num[Y_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[Y_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; } #endif } #endif #ifdef Z_STEP_PIN - if (st.step_outbits & (1 << Z_AXIS)) { + if (st.step_outbits & bit(Z_AXIS)) { #ifndef Z2_STEP_PIN // if not a ganged axis - RMT.conf_ch[Z_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[Z_rmt_chan_num].conf1.tx_start = 1; + RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; #else // it is a ganged axis if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) { - RMT.conf_ch[Z_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[Z_rmt_chan_num].conf1.tx_start = 1; + RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; } if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) { - RMT.conf_ch[Z2_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[Z2_rmt_chan_num].conf1.tx_start = 1; + RMT.conf_ch[rmt_chan_num[Z_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[Z_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; } #endif } #endif #ifdef A_STEP_PIN - if (st.step_outbits & (1 << A_AXIS)) { - RMT.conf_ch[A_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[A_rmt_chan_num].conf1.tx_start = 1; + if (st.step_outbits & bit(A_AXIS)) { +#ifndef A2_STEP_PIN // if not a ganged axis + RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; +#else // it is a ganged axis + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) { + RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; + } + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) { + RMT.conf_ch[rmt_chan_num[A_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[A_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; + } +#endif } #endif + #ifdef B_STEP_PIN - if (st.step_outbits & (1 << B_AXIS)) { - RMT.conf_ch[B_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[B_rmt_chan_num].conf1.tx_start = 1; + if (st.step_outbits & bit(B_AXIS)) { +#ifndef Z2_STEP_PIN // if not a ganged axis + RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; +#else // it is a ganged axis + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) { + RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; + } + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) { + RMT.conf_ch[rmt_chan_num[B_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[B_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; + } +#endif } #endif + #ifdef C_STEP_PIN - if (st.step_outbits & (1 << C_AXIS)) { - RMT.conf_ch[C_rmt_chan_num].conf1.mem_rd_rst = 1; - RMT.conf_ch[C_rmt_chan_num].conf1.tx_start = 1; + if (st.step_outbits & bit(C_AXIS)) { +#ifndef Z2_STEP_PIN // if not a ganged axis + RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; +#else // it is a ganged axis + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A)) { + RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; + } + if ((ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B)) { + RMT.conf_ch[rmt_chan_num[C_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; + RMT.conf_ch[rmt_chan_num[C_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; + } +#endif } #endif + + } #endif @@ -851,17 +679,21 @@ void st_go_idle() { // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active. Stepper_Timer_Stop(); busy = false; - bool pin_state = false; // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances. - if (((settings.stepper_idle_lock_time != 0xff) || sys_rt_exec_alarm || sys.state == STATE_SLEEP) && sys.state != STATE_HOMING) { + if (((stepper_idle_lock_time->get() != 0xff) || sys_rt_exec_alarm || sys.state == STATE_SLEEP) && sys.state != STATE_HOMING) { // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete // stop and not drift from residual inertial forces at the end of the last movement. - stepper_idle = true; // esp32 work around for disable in main loop - stepper_idle_counter = esp_timer_get_time() + (settings.stepper_idle_lock_time * 1000); // * 1000 because the time is in uSecs - //vTaskDelay(settings.stepper_idle_lock_time / portTICK_PERIOD_MS); // this probably does not work when called from ISR - //pin_state = true; + + if (sys.state == STATE_SLEEP || sys_rt_exec_alarm) { + motors_set_disable(true); + } else { + stepper_idle = true; // esp32 work around for disable in main loop + stepper_idle_counter = esp_timer_get_time() + (stepper_idle_lock_time->get() * 1000); // * 1000 because the time is in uSecs + // after idle countdown will be disabled in protocol loop + } } else - set_stepper_disable(pin_state); + motors_set_disable(false); + set_stepper_pins_on(0); } @@ -915,13 +747,13 @@ void st_generate_step_dir_invert_masks() { 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 = settings.step_invert_mask; - dir_port_invert_mask = settings.dir_invert_mask; + 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. @@ -1005,7 +837,7 @@ void st_prep_buffer() { prep.current_speed = sqrt(pl_block->entry_speed_sqr); - if (spindle->isRateAdjusted() ){ // settings.flags & BITFLAG_LASER_MODE) { + if (spindle->isRateAdjusted() ){ // laser_mode->get() { if (pl_block->condition & PL_COND_FLAG_SPINDLE_CCW) { // Pre-compute inverse programmed rate to speed up PWM updating per step segment. prep.inv_rate = 1.0 / pl_block->programmed_rate; @@ -1352,38 +1184,36 @@ float st_get_realtime_rate() { } void IRAM_ATTR Stepper_Timer_WritePeriod(uint64_t alarm_val) { +#ifdef USE_I2S_OUT_STREAM + // 1 tick = F_TIMERS / F_STEPPER_TIMER + // Pulse ISR is called for each tick of alarm_val. + i2s_out_set_pulse_period(alarm_val / 60); +#else timer_set_alarm_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, alarm_val); +#endif } void IRAM_ATTR Stepper_Timer_Start() { #ifdef ESP_DEBUG //Serial.println("ST Start"); #endif +#ifdef USE_I2S_OUT_STREAM + i2s_out_set_stepping(); +#else timer_set_counter_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, 0x00000000ULL); timer_start(STEP_TIMER_GROUP, STEP_TIMER_INDEX); TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN; +#endif } void IRAM_ATTR Stepper_Timer_Stop() { #ifdef ESP_DEBUG //Serial.println("ST Stop"); #endif +#ifdef USE_I2S_OUT_STREAM + i2s_out_set_passthrough(); +#else timer_pause(STEP_TIMER_GROUP, STEP_TIMER_INDEX); -} - - -void set_stepper_disable(uint8_t isOn) { // isOn = true // to disable -#ifdef USE_TRINAMIC_ENABLE - trinamic_stepper_enable(!isOn); -#endif - if (bit_istrue(settings.flags, BITFLAG_INVERT_ST_ENABLE)) { - isOn = !isOn; // Apply pin invert. - } -#ifdef USE_UNIPOLAR - unipolar_disable(isOn); -#endif -#ifdef STEPPERS_DISABLE_PIN - digitalWrite(STEPPERS_DISABLE_PIN, isOn); #endif } @@ -1394,7 +1224,7 @@ bool get_stepper_disable() { // returns true if steppers are disabled #else return false; // thery are never disabled if there is no pin defined #endif - if (bit_istrue(settings.flags, BITFLAG_INVERT_ST_ENABLE)) { + if (step_enable_invert->get()) { disabled = !disabled; // Apply pin invert. } return disabled; diff --git a/Grbl_Esp32/stepper.h b/Grbl_Esp32/stepper.h index 25d584ba..c3666af3 100644 --- a/Grbl_Esp32/stepper.h +++ b/Grbl_Esp32/stepper.h @@ -82,10 +82,6 @@ extern uint8_t ganged_mode; void IRAM_ATTR onSteppertimer(); void IRAM_ATTR onStepperOffTimer(); -#ifdef USE_RMT_STEPS - void initRMT(); -#endif - void stepper_init(); // Enable steppers, but cycle does not start unless called by motion control or realtime command. @@ -116,11 +112,8 @@ void st_update_plan_block_parameters(); float st_get_realtime_rate(); // disable (or enable) steppers via STEPPERS_DISABLE_PIN -void set_stepper_disable(uint8_t disable); bool get_stepper_disable(); // returns the state of the pin -void set_step_pin_on(uint8_t axis, uint8_t isOn); -void set_direction_pin_on(uint8_t axis, uint8_t isOn); void set_stepper_pins_on(uint8_t onMask); void set_direction_pins_on(uint8_t onMask); diff --git a/Grbl_Esp32/system.cpp b/Grbl_Esp32/system.cpp index 2da179ce..b74e889f 100644 --- a/Grbl_Esp32/system.cpp +++ b/Grbl_Esp32/system.cpp @@ -81,19 +81,19 @@ void system_ini() { // Renamed from system_init() due to conflict with esp32 fil // Setup USER_DIGITAL_PINs controlled by M62 and M63 #ifdef USER_DIGITAL_PIN_1 pinMode(USER_DIGITAL_PIN_1, OUTPUT); - sys_io_control(1 << 1, false); // turn off + sys_io_control(bit(1), false); // turn off #endif #ifdef USER_DIGITAL_PIN_2 pinMode(USER_DIGITAL_PIN_2, OUTPUT); - sys_io_control(1 << 2, false); // turn off + sys_io_control(bit(2), false); // turn off #endif #ifdef USER_DIGITAL_PIN_3 pinMode(USER_DIGITAL_PIN_3, OUTPUT); - sys_io_control(1 << 3, false); // turn off + sys_io_control(bit(3), false); // turn off #endif #ifdef USER_DIGITAL_PIN_4 pinMode(USER_DIGITAL_PIN_4, OUTPUT); - sys_io_control(1 << 4, false); // turn off + sys_io_control(bit(4), false); // turn off #endif } @@ -126,202 +126,6 @@ void IRAM_ATTR isr_control_inputs() { #endif } -// Executes user startup script, if stored. -void system_execute_startup(char* line) { - uint8_t n; - for (n = 0; n < N_STARTUP_LINE; n++) { - if (!(settings_read_startup_line(n, line))) { - line[0] = 0; - report_execute_startup_message(line, STATUS_SETTING_READ_FAIL, CLIENT_SERIAL); - } else { - if (line[0] != 0) { - uint8_t status_code = gc_execute_line(line, CLIENT_SERIAL); - report_execute_startup_message(line, status_code, CLIENT_SERIAL); - } - } - } -} - -// Directs and executes one line of formatted input from protocol_process. While mostly -// incoming streaming g-code blocks, this also executes Grbl internal commands, such as -// settings, initiating the homing cycle, and toggling switch states. This differs from -// the realtime command module by being susceptible to when Grbl is ready to execute the -// next line during a cycle, so for switches like block delete, the switch only effects -// the lines that are processed afterward, not necessarily real-time during a cycle, -// since there are motions already stored in the buffer. However, this 'lag' should not -// be an issue, since these commands are not typically used during a cycle. -uint8_t system_execute_line(char* line, uint8_t client) { - uint8_t char_counter = 1; - uint8_t helper_var = 0; // Helper variable - float parameter, value; - switch (line[char_counter]) { - case 0 : report_grbl_help(client); break; - case 'J' : // Jogging - // Execute only if in IDLE or JOG states. - if (sys.state != STATE_IDLE && sys.state != STATE_JOG) return (STATUS_IDLE_ERROR); - if (line[2] != '=') return (STATUS_INVALID_STATEMENT); - return (gc_execute_line(line, client)); // NOTE: $J= is ignored inside g-code parser and used to detect jog motions. - break; - case '$': case 'G': case 'C': case 'X': case '+': - if (line[2] != 0) return (STATUS_INVALID_STATEMENT); - switch (line[1]) { - case '$': case '+' : // Prints Grbl settings - if (sys.state & (STATE_CYCLE | STATE_HOLD)) return (STATUS_IDLE_ERROR); // Block during cycle. Takes too long to print. - else { - if (line[1] == '$') - report_grbl_settings(client, false); // entended settings depend on SHOW_EXTENDED_SETTINGS - else - report_grbl_settings(client, true); // force display of extended settings - } - break; - case 'G' : // Prints gcode parser state - // TODO: Move this to realtime commands for GUIs to request this data during suspend-state. - report_gcode_modes(client); - break; - case 'C' : // Set check g-code mode [IDLE/CHECK] - // Perform reset when toggling off. Check g-code mode should only work if Grbl - // is idle and ready, regardless of alarm locks. This is mainly to keep things - // simple and consistent. - if (sys.state == STATE_CHECK_MODE) { - mc_reset(); - report_feedback_message(MESSAGE_DISABLED); - } else { - if (sys.state) return (STATUS_IDLE_ERROR); // Requires no alarm mode. - sys.state = STATE_CHECK_MODE; - report_feedback_message(MESSAGE_ENABLED); - } - break; - case 'X' : // Disable alarm lock [ALARM] - if (sys.state == STATE_ALARM) { - // Block if safety door is ajar. - if (system_check_safety_door_ajar()) return (STATUS_CHECK_DOOR); - report_feedback_message(MESSAGE_ALARM_UNLOCK); - sys.state = STATE_IDLE; - // Don't run startup script. Prevents stored moves in startup from causing accidents. - } // Otherwise, no effect. - break; - } - break; - default : - // Block any system command that requires the state as IDLE/ALARM. (i.e. EEPROM, homing) - if (!(sys.state == STATE_IDLE || sys.state == STATE_ALARM)) return (STATUS_IDLE_ERROR); - switch (line[1]) { - case '#' : // Print Grbl NGC parameters - if (line[2] != 0) return (STATUS_INVALID_STATEMENT); - else report_ngc_parameters(client); - break; - case 'H' : // Perform homing cycle [IDLE/ALARM] $H - if (bit_isfalse(settings.flags, BITFLAG_HOMING_ENABLE)) return (STATUS_SETTING_DISABLED); - if (system_check_safety_door_ajar()) return (STATUS_CHECK_DOOR); // Block if safety door is ajar. - if (line[2] == 0) { - sys.state = STATE_HOMING; // Set system state variable - mc_homing_cycle(HOMING_CYCLE_ALL); -#ifdef HOMING_SINGLE_AXIS_COMMANDS - } else if (line[3] == 0) { - switch (line[2]) { - case 'X': sys.state = STATE_HOMING; mc_homing_cycle(HOMING_CYCLE_X); break; - case 'Y': sys.state = STATE_HOMING; mc_homing_cycle(HOMING_CYCLE_Y); break; - case 'Z': sys.state = STATE_HOMING; mc_homing_cycle(HOMING_CYCLE_Z); break; -#if (N_AXIS > 3) // make sure axis is defined - case 'A': sys.state = STATE_HOMING; mc_homing_cycle(HOMING_CYCLE_A); break; -#endif -#if (N_AXIS > 4) // make sure axis is defined - case 'B': sys.state = STATE_HOMING; mc_homing_cycle(HOMING_CYCLE_B); break; -#endif -#if (N_AXIS > 5) // make sure axis is defined - case 'C': sys.state = STATE_HOMING; mc_homing_cycle(HOMING_CYCLE_C); break; -#endif - default: - return (STATUS_INVALID_STATEMENT); - } -#endif - } else return (STATUS_INVALID_STATEMENT); - if (!sys.abort) { // Execute startup scripts after successful homing. - sys.state = STATE_IDLE; // Set to IDLE when complete. - st_go_idle(); // Set steppers to the settings idle state before returning. - if (line[2] == 0) system_execute_startup(line); - } - break; - case 'S' : // Puts Grbl to sleep [IDLE/ALARM] - if ((line[2] != 'L') || (line[3] != 'P') || (line[4] != 0)) return (STATUS_INVALID_STATEMENT); - system_set_exec_state_flag(EXEC_SLEEP); // Set to execute sleep mode immediately - break; - case 'I' : // Print or store build info. [IDLE/ALARM] - if (line[++char_counter] == 0) { - settings_read_build_info(line); - report_build_info(line, client); -#ifdef ENABLE_BUILD_INFO_WRITE_COMMAND - } else { // Store startup line [IDLE/ALARM] - if (line[char_counter++] != '=') return (STATUS_INVALID_STATEMENT); - helper_var = char_counter; // Set helper variable as counter to start of user info line. - do { - line[char_counter - helper_var] = line[char_counter]; - } while (line[char_counter++] != 0); - settings_store_build_info(line); -#endif - } - break; - case 'R' : // Restore defaults [IDLE/ALARM] - if ((line[2] != 'S') || (line[3] != 'T') || (line[4] != '=') || (line[6] != 0)) return (STATUS_INVALID_STATEMENT); - switch (line[5]) { -#ifdef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS - case '$': settings_restore(SETTINGS_RESTORE_DEFAULTS); break; -#endif -#ifdef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS - case '#': settings_restore(SETTINGS_RESTORE_PARAMETERS); break; -#endif -#ifdef ENABLE_RESTORE_EEPROM_WIPE_ALL - case '*': settings_restore(SETTINGS_RESTORE_ALL); break; -#endif -#if defined(ENABLE_BLUETOOTH) || defined(ENABLE_WIFI) - case '@': settings_restore(SETTINGS_RESTORE_WIFI_SETTINGS); break; -#endif - default: return (STATUS_INVALID_STATEMENT); - } - report_feedback_message(MESSAGE_RESTORE_DEFAULTS); - mc_reset(); // Force reset to ensure settings are initialized correctly. - break; - case 'N' : // Startup lines. [IDLE/ALARM] - if (line[++char_counter] == 0) { // Print startup lines - for (helper_var = 0; helper_var < N_STARTUP_LINE; helper_var++) { - if (!(settings_read_startup_line(helper_var, line))) - report_status_message(STATUS_SETTING_READ_FAIL, CLIENT_SERIAL); - else - report_startup_line(helper_var, line, client); - } - break; - } else { // Store startup line [IDLE Only] Prevents motion during ALARM. - if (sys.state != STATE_IDLE) return (STATUS_IDLE_ERROR); // Store only when idle. - helper_var = true; // Set helper_var to flag storing method. - // No break. Continues into default: to read remaining command characters. - } - default : // Storing setting methods [IDLE/ALARM] - if (!read_float(line, &char_counter, ¶meter)) return (STATUS_BAD_NUMBER_FORMAT); - if (line[char_counter++] != '=') return (STATUS_INVALID_STATEMENT); - if (helper_var) { // Store startup line - // Prepare sending gcode block to gcode parser by shifting all characters - helper_var = char_counter; // Set helper variable as counter to start of gcode block - do { - line[char_counter - helper_var] = line[char_counter]; - } while (line[char_counter++] != 0); - // Execute gcode block to ensure block is valid. - helper_var = gc_execute_line(line, CLIENT_SERIAL); // Set helper_var to returned status code. - if (helper_var) return (helper_var); - else { - helper_var = trunc(parameter); // Set helper_var to int value of parameter - settings_store_startup_line(helper_var, line); - } - } else { // Store global setting. - if (!read_float(line, &char_counter, &value)) return (STATUS_BAD_NUMBER_FORMAT); - if ((line[char_counter] != 0) || (parameter > 255)) return (STATUS_INVALID_STATEMENT); - return (settings_store_global_setting((uint8_t)parameter, value)); - } - } - } - return (STATUS_OK); // If '$' command makes it to here, then everything's ok. -} - - // Returns if safety door is ajar(T) or closed(F), based on pin state. uint8_t system_check_safety_door_ajar() { #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN @@ -402,15 +206,16 @@ void system_flag_wco_change() { // serves as a central place to compute the transformation. float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx) { float pos; + float steps_per_mm = axis_settings[idx]->steps_per_mm->get(); #ifdef COREXY if (idx == X_AXIS) - pos = (float)system_convert_corexy_to_x_axis_steps(steps) / settings.steps_per_mm[idx]; + pos = (float)system_convert_corexy_to_x_axis_steps(steps) / steps_per_mm; else if (idx == Y_AXIS) - pos = (float)system_convert_corexy_to_y_axis_steps(steps) / settings.steps_per_mm[idx]; + pos = (float)system_convert_corexy_to_y_axis_steps(steps) / steps_per_mm; else - pos = steps[idx] / settings.steps_per_mm[idx]; + pos = steps[idx] / steps_per_mm; #else - pos = steps[idx] / settings.steps_per_mm[idx]; + pos = steps[idx] / steps_per_mm; #endif return (pos); } @@ -422,26 +227,24 @@ void system_convert_array_steps_to_mpos(float* position, int32_t* steps) { return; } - - // Checks and reports if target array exceeds machine travel limits. uint8_t system_check_travel_limits(float* target) { uint8_t idx; for (idx = 0; idx < N_AXIS; idx++) { + float travel = axis_settings[idx]->max_travel->get(); #ifdef HOMING_FORCE_SET_ORIGIN + uint8_t mask = homing_dir_mask->get(); // When homing forced set origin is enabled, soft limits checks need to account for directionality. - // NOTE: max_travel is stored as negative - if (bit_istrue(settings.homing_dir_mask, bit(idx))) { - if (target[idx] < 0 || target[idx] > -settings.max_travel[idx]) return (true); + if (bit_istrue(mask, bit(idx))) { + if (target[idx] < 0 || target[idx] > travel) return (true); } else { - if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) return (true); + if (target[idx] > 0 || target[idx] < -travel) return (true); } #else - // NOTE: max_travel is stored as negative #ifdef HOMING_FORCE_POSITIVE_SPACE - if (target[idx] < 0 || target[idx] > -settings.max_travel[idx]) return (true); + if (target[idx] < 0 || target[idx] > travel) return (true); #else - if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) return (true); + if (target[idx] > 0 || target[idx] < -travel) return (true); #endif #endif } @@ -493,17 +296,6 @@ uint8_t system_control_get_state() { return (control_state); } -// Returns limit pin mask according to Grbl internal axis indexing. -uint8_t get_limit_pin_mask(uint8_t axis_idx) { - if (axis_idx == X_AXIS) return ((1 << X_LIMIT_BIT)); - if (axis_idx == Y_AXIS) return ((1 << Y_LIMIT_BIT)); - if (axis_idx == Z_AXIS) return ((1 << Z_LIMIT_BIT)); - if (axis_idx == A_AXIS) return ((1 << A_LIMIT_BIT)); - if (axis_idx == B_AXIS) return ((1 << B_LIMIT_BIT)); - if (axis_idx == C_AXIS) return ((1 << C_LIMIT_BIT)); - return 0; -} - // execute the function of the control pin void system_exec_control_pin(uint8_t pin) { if (bit_istrue(pin, CONTROL_PIN_INDEX_RESET)) { @@ -550,25 +342,25 @@ int32_t system_convert_corexy_to_y_axis_steps(int32_t* steps) { void sys_io_control(uint8_t io_num_mask, bool turnOn) { protocol_buffer_synchronize(); #ifdef USER_DIGITAL_PIN_1 - if (io_num_mask & 1 << 1) { + if (io_num_mask & bit(1)) { digitalWrite(USER_DIGITAL_PIN_1, turnOn); return; } #endif #ifdef USER_DIGITAL_PIN_2 - if (io_num_mask & 1 << 2) { + if (io_num_mask & bit(2)) { digitalWrite(USER_DIGITAL_PIN_2, turnOn); return; } #endif #ifdef USER_DIGITAL_PIN_3 - if (io_num_mask & 1 << 3) { + if (io_num_mask & bit(3)) { digitalWrite(USER_DIGITAL_PIN_3, turnOn); return; } #endif #ifdef USER_DIGITAL_PIN_4 - if (io_num_mask & 1 << 4) { + if (io_num_mask & bit(4)) { digitalWrite(USER_DIGITAL_PIN_4, turnOn); return; } diff --git a/Grbl_Esp32/system.h b/Grbl_Esp32/system.h index 3f545076..1a36f6b1 100644 --- a/Grbl_Esp32/system.h +++ b/Grbl_Esp32/system.h @@ -22,6 +22,7 @@ #define system_h #include "grbl.h" #include "tdef.h" +#include "commands.h" // Define global system variables typedef struct { @@ -153,8 +154,6 @@ extern system_t sys; #define SPINDLE_STOP_OVR_RESTORE bit(2) #define SPINDLE_STOP_OVR_RESTORE_CYCLE bit(3) -#define UNDEFINED_PIN 255 // Can be used to show a pin has no i/O assigned - // NOTE: These position variables may need to be declared as volatiles, if problems arise. extern int32_t sys_position[N_AXIS]; // Real-time machine (aka home) position vector in steps. extern int32_t sys_probe_position[N_AXIS]; // Last probe position in machine coordinates and steps. @@ -193,8 +192,10 @@ void system_clear_exec_accessory_overrides(); // Execute the startup script lines stored in EEPROM upon initialization void system_execute_startup(char* line); -uint8_t system_execute_line(char* line, uint8_t client); - +uint8_t execute_line(char* line, uint8_t client, auth_t auth_level); +uint8_t system_execute_line(char* line, ESPResponseStream*, auth_t); +uint8_t system_execute_line(char* line, uint8_t client, auth_t); +uint8_t do_command_or_setting(const char *key, char *value, auth_t auth_level, ESPResponseStream*); void system_flag_wco_change(); // Returns machine position of axis 'idx'. Must be sent a 'step' array. @@ -205,7 +206,6 @@ void system_convert_array_steps_to_mpos(float* position, int32_t* steps); // Checks and reports if target array exceeds machine travel limits. uint8_t system_check_travel_limits(float* target); -uint8_t get_limit_pin_mask(uint8_t axis_idx); // Special handlers for setting and clearing Grbl's real-time execution flags. void system_set_exec_state_flag(uint8_t mask); @@ -231,4 +231,4 @@ void sys_io_control(uint8_t io_num_mask, bool turnOn); int8_t sys_get_next_RMT_chan_num(); int8_t sys_get_next_PWM_chan_num(); -#endif \ No newline at end of file +#endif diff --git a/Grbl_Esp32/telnet_server.cpp b/Grbl_Esp32/telnet_server.cpp index 3a2d2d5d..13d36667 100644 --- a/Grbl_Esp32/telnet_server.cpp +++ b/Grbl_Esp32/telnet_server.cpp @@ -20,21 +20,15 @@ #ifdef ARDUINO_ARCH_ESP32 -#include "config.h" +#include "grbl.h" #if defined (ENABLE_WIFI) && defined (ENABLE_TELNET) #include "wifiservices.h" -#include "grbl.h" - #include "telnet_server.h" #include "wificonfig.h" #include -#include -#include "report.h" -#include "commands.h" - Telnet_Server telnet_server; bool Telnet_Server::_setupdone = false; @@ -53,19 +47,16 @@ Telnet_Server::~Telnet_Server() { end(); } - bool Telnet_Server::begin() { bool no_error = true; end(); - Preferences prefs; _RXbufferSize = 0; _RXbufferpos = 0;; - prefs.begin(NAMESPACE, true); - int8_t penabled = prefs.getChar(TELNET_ENABLE_ENTRY, DEFAULT_TELNET_STATE); - //Get telnet port - _port = prefs.getUShort(TELNET_PORT_ENTRY, DEFAULT_TELNETSERVER_PORT); - prefs.end(); - if (penabled == 0) return false; + if (telnet_enable->get() == 0) { + return false; + } + _port = telnet_port->get(); + //create instance _telnetserver = new WiFiServer(_port, MAX_TLNT_CLIENTS); _telnetserver->setNoDelay(true); diff --git a/Grbl_Esp32/tests/raster_tree.nc b/Grbl_Esp32/tests/raster_tree.nc index 41eefbd6..8a560830 100644 --- a/Grbl_Esp32/tests/raster_tree.nc +++ b/Grbl_Esp32/tests/raster_tree.nc @@ -1,6 +1,6 @@ G0 X0 Y0 M3 S0 -F1000 +G1 F1000 G0 X71 Y0.333 S0 G1 X70.667 S43 X70.333 S65 @@ -54268,3 +54268,4 @@ G1 X61.667 S4 X61.333 S12 X61 S13 M5 +G0 X0 Y0 \ No newline at end of file diff --git a/Grbl_Esp32/web_server.cpp b/Grbl_Esp32/web_server.cpp index c557d789..18678f6c 100644 --- a/Grbl_Esp32/web_server.cpp +++ b/Grbl_Esp32/web_server.cpp @@ -20,20 +20,16 @@ #ifdef ARDUINO_ARCH_ESP32 -#include "config.h" +#include "grbl.h" #if defined (ENABLE_WIFI) && defined (ENABLE_HTTP) #include "wifiservices.h" -#include "grbl.h" - -#include "commands.h" #include "espresponse.h" #include "serial2socket.h" #include "web_server.h" #include -#include "wificonfig.h" #include #include #include @@ -41,8 +37,6 @@ #include #include "grbl_sd.h" #endif -#include -#include "report.h" #include #include #include @@ -115,13 +109,11 @@ bool Web_Server::begin(){ bool no_error = true; _setupdone = false; - Preferences prefs; - prefs.begin(NAMESPACE, true); - int8_t penabled = prefs.getChar(HTTP_ENABLE_ENTRY, DEFAULT_HTTP_STATE); - //Get http port - _port = prefs.getUShort(HTTP_PORT_ENTRY, DEFAULT_WEBSERVER_PORT); - prefs.end(); - if (penabled == 0) return false; + if (http_enable->get() == 0) { + return false; + } + _port = http_port->get(); + //create instance _webserver= new WebServer(_port); #ifdef ENABLE_AUTHENTICATION @@ -468,8 +460,7 @@ bool Web_Server::is_realtime_cmd(char c){ return false; } -//Handle web command query and send answer////////////////////////////// -void Web_Server::handle_web_command () +void Web_Server::_handle_web_command (bool silent) { //to save time if already disconnected //if (_webserver->hasArg ("PAGEID") ) { @@ -480,14 +471,12 @@ void Web_Server::handle_web_command () // } // } //} - level_authenticate_type auth_level = is_authenticated(); + auth_t auth_level = is_authenticated(); String cmd = ""; - if (_webserver->hasArg ("plain") || _webserver->hasArg ("commandText") ) { - if (_webserver->hasArg ("plain") ) { - cmd = _webserver->arg ("plain"); - } else { - cmd = _webserver->arg ("commandText"); - } + if (_webserver->hasArg ("plain")) { + cmd = _webserver->arg ("plain"); + } else if (_webserver->hasArg ("commandText")) { + cmd = _webserver->arg ("commandText"); } else { _webserver->send (200, "text/plain", "Invalid command"); return; @@ -496,105 +485,26 @@ void Web_Server::handle_web_command () cmd.trim(); int ESPpos = cmd.indexOf ("[ESP"); if (ESPpos > -1) { - //is there the second part? - int ESPpos2 = cmd.indexOf ("]", ESPpos); - if (ESPpos2 > -1) { - //Split in command and parameters - String cmd_part1 = cmd.substring (ESPpos + 4, ESPpos2); - String cmd_part2 = ""; - //only [ESP800] is allowed login free if authentication is enabled - if ( (auth_level == LEVEL_GUEST) && (cmd_part1.toInt() != 800) ) { - _webserver->send (401, "text/plain", "Authentication failed!\n"); - return; - } - //is there space for parameters? - if (ESPpos2 < cmd.length() ) { - cmd_part2 = cmd.substring (ESPpos2 + 1); - } - //if command is a valid number then execute command - if (cmd_part1.toInt() >= 0) { - ESPResponseStream espresponse(_webserver); - //commmand is web only - COMMANDS::execute_internal_command (cmd_part1.toInt(), cmd_part2, auth_level, &espresponse); - //flush - espresponse.flush(); - } - //if not is not a valid [ESPXXX] command - } - } else { //execute GCODE - if (auth_level == LEVEL_GUEST) { - _webserver->send (401, "text/plain", "Authentication failed!\n"); - return; - } - //Instead of send several commands one by one by web / send full set and split here - String scmd; - String res = ""; - uint8_t sindex = 0; - scmd = get_Splited_Value(cmd,'\n', sindex); - while ( scmd != "" ){ - if ((scmd.length() == 2) && (scmd[0] == 0xC2)){ - scmd[0]=scmd[1]; - scmd.remove(1,1); - } - if (scmd.length() > 1)scmd += "\n"; - else if (!is_realtime_cmd(scmd[0]) )scmd += "\n"; - if (!Serial2Socket.push(scmd.c_str()))res = "Error"; - sindex++; - scmd = get_Splited_Value(cmd,'\n', sindex); - } - _webserver->send (200, "text/plain", res.c_str()); - } -} -//Handle web command query and send answer////////////////////////////// -void Web_Server::handle_web_command_silent () -{ - //to save time if already disconnected - //if (_webserver->hasArg ("PAGEID") ) { - // if (_webserver->arg ("PAGEID").length() > 0 ) { - // if (_webserver->arg ("PAGEID").toInt() != _id_connection) { - // _webserver->send (200, "text/plain", "Invalid command"); - // return; - // } - // } - //} - level_authenticate_type auth_level = is_authenticated(); - String cmd = ""; - if (_webserver->hasArg ("plain") || _webserver->hasArg ("commandText") ) { - if (_webserver->hasArg ("plain") ) { - cmd = _webserver->arg ("plain"); + char line[256]; + strncpy(line, cmd.c_str(), 255); + ESPResponseStream* espresponse = silent ? NULL : new ESPResponseStream(_webserver); + err_t err = system_execute_line(line, espresponse, auth_level); + String answer; + if (err == STATUS_OK) { + answer = "ok"; } else { - cmd = _webserver->arg ("commandText"); + const char* msg = errorString(err); + answer = "Error: "; + if (msg) { + answer += msg; + } else { + answer += err; + } } - } else { - _webserver->send (200, "text/plain", "Invalid command"); - return; - } - //if it is internal command [ESPXXX] - cmd.trim(); - int ESPpos = cmd.indexOf ("[ESP"); - if (ESPpos > -1) { - //is there the second part? - int ESPpos2 = cmd.indexOf ("]", ESPpos); - if (ESPpos2 > -1) { - //Split in command and parameters - String cmd_part1 = cmd.substring (ESPpos + 4, ESPpos2); - String cmd_part2 = ""; - //only [ESP800] is allowed login free if authentication is enabled - if ( (auth_level == LEVEL_GUEST) && (cmd_part1.toInt() != 800) ) { - _webserver->send (401, "text/plain", "Authentication failed!\n"); - return; - } - //is there space for parameters? - if (ESPpos2 < cmd.length() ) { - cmd_part2 = cmd.substring (ESPpos2 + 1); - } - //if command is a valid number then execute command - if (cmd_part1.toInt() >= 0) { - //commmand is web only - if(COMMANDS::execute_internal_command (cmd_part1.toInt(), cmd_part2, auth_level, NULL)) _webserver->send (200, "text/plain", "ok"); - else _webserver->send (200, "text/plain", "error"); - } - //if not is not a valid [ESPXXX] command + if (silent || !espresponse->anyOutput()) { + _webserver->send (err ? 401 : 200, "text/plain", answer.c_str()); + } else { + espresponse->flush(); } } else { //execute GCODE if (auth_level == LEVEL_GUEST) { @@ -603,21 +513,34 @@ void Web_Server::handle_web_command_silent () } //Instead of send several commands one by one by web / send full set and split here String scmd; + const char *res = ""; uint8_t sindex = 0; - scmd = get_Splited_Value(cmd,'\n', sindex); - String res = ""; - while ( scmd != "" ){ - if (scmd.length() > 1)scmd+="\n"; - else if (!is_realtime_cmd(scmd[0]) )scmd+="\n"; - if (!Serial2Socket.push(scmd.c_str()))res = "Error"; - sindex++; - scmd = get_Splited_Value(cmd,'\n', sindex); + // TODO Settings - this is very inefficient. get_Splited_Value() is O(n^2) + // when it could easily be O(n). Also, it would be just as easy to push + // the entire string into Serial2Socket and pull off lines from there. + for (uint8_t sindex = 0; + (scmd = get_Splited_Value(cmd,'\n', sindex)) != ""; + sindex++) { + // 0xC2 is an HTML encoding prefix that, in UTF-8 mode, + // precede 0x90 and 0xa0-0bf, which are GRBL realtime commands. + // There are other encodings for 0x91-0x9f, so I am not sure + // how - or whether - those commands work. + // Ref: https://www.w3schools.com/tags/ref_urlencode.ASP + if (!silent && (scmd.length() == 2) && (scmd[0] == 0xC2)) { + scmd[0]=scmd[1]; + scmd.remove(1,1); + } + if (scmd.length() > 1) + scmd += "\n"; + else if (!is_realtime_cmd(scmd[0]) ) + scmd += "\n"; + if (!Serial2Socket.push(scmd.c_str())) + res = "Error"; } - _webserver->send (200, "text/plain", res.c_str()); + _webserver->send (200, "text/plain", res); } } - //login status check void Web_Server::handle_login() { @@ -645,7 +568,7 @@ void Web_Server::handle_login() return; } - level_authenticate_type auth_level = is_authenticated(); + auth_t auth_level = is_authenticated(); if (auth_level == LEVEL_GUEST) auths = "guest"; else if (auth_level == LEVEL_USER) auths = "user"; else if (auth_level == LEVEL_ADMIN) auths = "admin"; @@ -665,16 +588,8 @@ void Web_Server::handle_login() if (msg_alert_error == false) { //Password sPassword = _webserver->arg("PASSWORD"); - String sadminPassword; - - Preferences prefs; - prefs.begin(NAMESPACE, true); - String defV = DEFAULT_ADMIN_PWD; - sadminPassword = prefs.getString(ADMIN_PWD_ENTRY, defV); - String suserPassword; - defV = DEFAULT_USER_PWD; - suserPassword = prefs.getString(USER_PWD_ENTRY, defV); - prefs.end(); + String sadminPassword = admin_password->get(); + String suserPassword = user_password->get(); if(!(((sUser == DEFAULT_ADMIN_LOGIN) && (strcmp(sPassword.c_str(),sadminPassword.c_str()) == 0)) || ((sUser == DEFAULT_USER_LOGIN) && (strcmp(sPassword.c_str(),suserPassword.c_str()) == 0)))) { @@ -691,19 +606,18 @@ void Web_Server::handle_login() //change password if (_webserver->hasArg("PASSWORD") && _webserver->hasArg("USER") && _webserver->hasArg("NEWPASSWORD") && (msg_alert_error==false) ) { String newpassword = _webserver->arg("NEWPASSWORD"); - if (COMMANDS::isLocalPasswordValid(newpassword.c_str())) { - String spos; - if(sUser == DEFAULT_ADMIN_LOGIN) spos = ADMIN_PWD_ENTRY; - else spos = USER_PWD_ENTRY; - - Preferences prefs; - prefs.begin(NAMESPACE, false); - if (prefs.putString(spos.c_str(), newpassword) != newpassword.length()) { + if (COMMANDS::isLocalPasswordValid((char *)newpassword.c_str())) { + err_t err; + if (sUser == DEFAULT_ADMIN_LOGIN) { + err = admin_password->setStringValue((char *)newpassword.c_str()); + } else { + err = user_password->setStringValue((char *)newpassword.c_str()); + } + if (err) { msg_alert_error = true; smsg = "Error: Cannot apply changes"; code = 500; } - prefs.end(); } else { msg_alert_error=true; smsg = "Error: Incorrect password"; @@ -711,7 +625,7 @@ void Web_Server::handle_login() } } if ((code == 200) || (code == 500)) { - level_authenticate_type current_auth_level; + auth_t current_auth_level; if(sUser == DEFAULT_ADMIN_LOGIN) { current_auth_level = LEVEL_ADMIN; } else if(sUser == DEFAULT_USER_LOGIN){ @@ -788,7 +702,7 @@ void Web_Server::handle_login() //SPIFFS files list and file commands void Web_Server::handleFileList () { - level_authenticate_type auth_level = is_authenticated(); + auth_t auth_level = is_authenticated(); if (auth_level == LEVEL_GUEST) { _upload_status = UPLOAD_STATUS_NONE; _webserver->send (401, "text/plain", "Authentication failed!\n"); @@ -1017,7 +931,7 @@ void Web_Server::SPIFFSFileupload () static String filename; static File fsUploadFile = (File)0; //get authentication status - level_authenticate_type auth_level= is_authenticated(); + auth_t auth_level= is_authenticated(); //Guest cannot upload - only admin if (auth_level == LEVEL_GUEST) { _upload_status = UPLOAD_STATUS_FAILED; @@ -1140,7 +1054,7 @@ void Web_Server::SPIFFSFileupload () //Web Update handler void Web_Server::handleUpdate () { - level_authenticate_type auth_level = is_authenticated(); + auth_t auth_level = is_authenticated(); if (auth_level != LEVEL_ADMIN) { _upload_status = UPLOAD_STATUS_NONE; _webserver->send (403, "text/plain", "Not allowed, log in first!\n"); @@ -1399,6 +1313,7 @@ void Web_Server::handle_direct_SDFileList() list_files = false; } } + // TODO Settings - consider using the JSONEncoder class String jsonfile = "{" ; jsonfile+="\"files\":["; @@ -1688,6 +1603,10 @@ void Web_Server::handle_Websocket_Event(uint8_t num, uint8_t type, uint8_t * pay } +// The separator that is passed in to this function is always '\n' +// The string that is returned does not contain the separator +// The calling code adds back the separator, unless the string is +// a one-character realtime command. String Web_Server::get_Splited_Value(String data, char separator, int index) { int found = 0; @@ -1745,7 +1664,7 @@ String Web_Server::getContentType (String filename) } //check authentification -level_authenticate_type Web_Server::is_authenticated() +auth_t Web_Server::is_authenticated() { #ifdef ENABLE_AUTHENTICATION if (_webserver->hasHeader ("Cookie") ) { @@ -1848,7 +1767,7 @@ auth_ip * Web_Server::GetAuth (IPAddress ip, const char * sessionID) } //Review all IP to reset timers -level_authenticate_type Web_Server::ResetAuthIP (IPAddress ip, const char * sessionID) +auth_t Web_Server::ResetAuthIP (IPAddress ip, const char * sessionID) { auth_ip * current = _head; auth_ip * previous = NULL; @@ -1873,7 +1792,7 @@ level_authenticate_type Web_Server::ResetAuthIP (IPAddress ip, const char * sess if (strcmp (sessionID, current->sessionID) == 0) { //reset time current->last_time = millis(); - return (level_authenticate_type) current->level; + return (auth_t) current->level; } } previous = current; diff --git a/Grbl_Esp32/web_server.h b/Grbl_Esp32/web_server.h index 7c0f8e32..9028898f 100644 --- a/Grbl_Esp32/web_server.h +++ b/Grbl_Esp32/web_server.h @@ -31,13 +31,12 @@ class WebServer; #ifdef ENABLE_AUTHENTICATION struct auth_ip { IPAddress ip; - level_authenticate_type level; + auth_t level; char userID[17]; char sessionID[17]; uint32_t last_time; auth_ip* _next; }; - #endif class Web_Server { @@ -58,7 +57,7 @@ class Web_Server { static uint8_t _upload_status; static String getContentType(String filename); static String get_Splited_Value(String data, char separator, int index); - static level_authenticate_type is_authenticated(); + static auth_t is_authenticated(); #ifdef ENABLE_AUTHENTICATION static auth_ip* _head; static uint8_t _nb_ip; @@ -66,7 +65,7 @@ class Web_Server { static char* create_session_ID(); static bool ClearAuthIP(IPAddress ip, const char* sessionID); static auth_ip* GetAuth(IPAddress ip, const char* sessionID); - static level_authenticate_type ResetAuthIP(IPAddress ip, const char* sessionID); + static auth_t ResetAuthIP(IPAddress ip, const char* sessionID); #endif #ifdef ENABLE_SSDP static void handle_SSDP(); @@ -74,8 +73,9 @@ class Web_Server { static void handle_root(); static void handle_login(); static void handle_not_found(); - static void handle_web_command(); - static void handle_web_command_silent(); + static void _handle_web_command(bool); + static void handle_web_command() { _handle_web_command(false); } + static void handle_web_command_silent() { _handle_web_command(true); } static void handle_Websocket_Event(uint8_t num, uint8_t type, uint8_t* payload, size_t length); static void SPIFFSFileupload(); static void handleFileList(); diff --git a/Grbl_Esp32/wificonfig.cpp b/Grbl_Esp32/wificonfig.cpp index 15881cb1..80857f7f 100644 --- a/Grbl_Esp32/wificonfig.cpp +++ b/Grbl_Esp32/wificonfig.cpp @@ -20,7 +20,7 @@ #ifdef ARDUINO_ARCH_ESP32 -#include "config.h" +#include "grbl.h" #ifdef ENABLE_WIFI @@ -29,11 +29,8 @@ #include #include #include -#include -#include "wificonfig.h" #include "wifiservices.h" -#include "commands.h" -#include "report.h" + WiFiConfig wifi_config; @@ -116,8 +113,7 @@ String WiFiConfig::IP_string_from_int(uint32_t ip_int) { bool WiFiConfig::isHostnameValid(const char* hostname) { //limited size char c; - if (strlen(hostname) > MAX_HOSTNAME_LENGTH || strlen(hostname) < MIN_HOSTNAME_LENGTH) - return false; + // length is checked automatically by string setting //only letter and digit for (int i = 0; i < strlen(hostname); i++) { c = hostname[i]; @@ -137,8 +133,7 @@ bool WiFiConfig::isHostnameValid(const char* hostname) { bool WiFiConfig::isSSIDValid(const char* ssid) { //limited size //char c; - if (strlen(ssid) > MAX_SSID_LENGTH || strlen(ssid) < MIN_SSID_LENGTH) - return false; + // length is checked automatically by string setting //only printable for (int i = 0; i < strlen(ssid); i++) { if (!isPrintable(ssid[i])) @@ -154,8 +149,7 @@ bool WiFiConfig::isSSIDValid(const char* ssid) { bool WiFiConfig::isPasswordValid(const char* password) { if (strlen(password) == 0) return true; //open network //limited size - if ((strlen(password) > MAX_PASSWORD_LENGTH) || (strlen(password) < MIN_PASSWORD_LENGTH)) - return false; + // length is checked automatically by string setting //no space allowed ? /* for (int i = 0; i < strlen (password); i++) if (password[i] == ' ') { @@ -268,8 +262,6 @@ bool WiFiConfig::ConnectSTA2AP() { */ bool WiFiConfig::StartSTA() { - String defV; - Preferences prefs; //stop active service wifi_services.end(); //Sanity check @@ -278,28 +270,17 @@ bool WiFiConfig::StartSTA() { WiFi.enableAP(false); WiFi.mode(WIFI_STA); //Get parameters for STA - prefs.begin(NAMESPACE, true); - defV = DEFAULT_HOSTNAME; - String h = prefs.getString(HOSTNAME_ENTRY, defV); + String h = wifi_hostname->get(); WiFi.setHostname(h.c_str()); //SSID - defV = DEFAULT_STA_SSID; - String SSID = prefs.getString(STA_SSID_ENTRY, defV); - if (SSID.length() == 0)SSID = DEFAULT_STA_SSID; + String SSID = wifi_sta_ssid->get(); + if (SSID.length() == 0) SSID = DEFAULT_STA_SSID; //password - defV = DEFAULT_STA_PWD; - String password = prefs.getString(STA_PWD_ENTRY, defV); - int8_t IP_mode = prefs.getChar(STA_IP_MODE_ENTRY, DHCP_MODE); - //IP - defV = DEFAULT_STA_IP; - int32_t IP = prefs.getInt(STA_IP_ENTRY, IP_int_from_string(defV)); - //GW - defV = DEFAULT_STA_GW; - int32_t GW = prefs.getInt(STA_GW_ENTRY, IP_int_from_string(defV)); - //MK - defV = DEFAULT_STA_MK; - int32_t MK = prefs.getInt(STA_MK_ENTRY, IP_int_from_string(defV)); - prefs.end(); + String password = wifi_sta_password->get(); + int8_t IP_mode = wifi_sta_mode->get(); + int32_t IP = wifi_sta_ip->get(); + int32_t GW = wifi_sta_gateway->get(); + int32_t MK = wifi_sta_netmask->get(); //if not DHCP if (IP_mode != DHCP_MODE) { IPAddress ip(IP), mask(MK), gateway(GW); @@ -320,8 +301,6 @@ bool WiFiConfig::StartSTA() { */ bool WiFiConfig::StartAP() { - String defV; - Preferences prefs; //stop active services wifi_services.end(); //Sanity check @@ -330,23 +309,16 @@ bool WiFiConfig::StartAP() { WiFi.enableSTA(false); WiFi.mode(WIFI_AP); //Get parameters for AP - prefs.begin(NAMESPACE, true); //SSID - defV = DEFAULT_AP_SSID; - String SSID = prefs.getString(AP_SSID_ENTRY, defV); - if (SSID.length() == 0)SSID = DEFAULT_AP_SSID; - //password - defV = DEFAULT_AP_PWD; - String password = prefs.getString(AP_PWD_ENTRY, defV); - //channel - int8_t channel = prefs.getChar(AP_CHANNEL_ENTRY, DEFAULT_AP_CHANNEL); - if (channel == 0)channel = DEFAULT_AP_CHANNEL; - //IP - defV = DEFAULT_AP_IP; - int32_t IP = prefs.getInt(AP_IP_ENTRY, IP_int_from_string(defV)); - if (IP == 0) - IP = IP_int_from_string(defV); - prefs.end(); + String SSID = wifi_ap_ssid->get(); + if (SSID.length() == 0) SSID = DEFAULT_AP_SSID; + + String password = wifi_ap_password->get(); + + int8_t channel = wifi_ap_channel->get(); + if (channel == 0) channel = DEFAULT_AP_CHANNEL; + + int32_t IP = wifi_ap_ip->get(); IPAddress ip(IP); IPAddress mask; mask.fromString(DEFAULT_AP_MK); @@ -381,7 +353,6 @@ void WiFiConfig::StopWiFi() { * begin WiFi setup */ void WiFiConfig::begin() { - Preferences prefs; //stop active services wifi_services.end(); //setup events @@ -390,26 +361,21 @@ void WiFiConfig::begin() { WiFi.onEvent(WiFiConfig::WiFiEvent); _events_registered = true; } - //open preferences as read-only - prefs.begin(NAMESPACE, true); //Get hostname - String defV = DEFAULT_HOSTNAME; - _hostname = prefs.getString(HOSTNAME_ENTRY, defV); - int8_t wifiMode = prefs.getChar(ESP_RADIO_MODE, DEFAULT_RADIO_MODE); + _hostname = wifi_hostname->get(); + int8_t wifiMode = wifi_radio_mode->get(); if (wifiMode == ESP_WIFI_AP) { StartAP(); //start services wifi_services.begin(); } else if (wifiMode == ESP_WIFI_STA) { if (!StartSTA()) { - defV = DEFAULT_STA_SSID; - grbl_sendf(CLIENT_ALL, "[MSG:Cannot connect to %s]\r\n", prefs.getString(STA_SSID_ENTRY, defV).c_str()); + grbl_sendf(CLIENT_ALL, "[MSG:Cannot connect to %s]\r\n", wifi_sta_ssid->get()); StartAP(); } //start services wifi_services.begin(); } else WiFi.mode(WIFI_OFF); - prefs.end(); } /** @@ -423,75 +389,16 @@ void WiFiConfig::end() { * Reset ESP */ void WiFiConfig::reset_settings() { - Preferences prefs; - prefs.begin(NAMESPACE, false); - String sval; - int8_t bbuf; - int16_t ibuf; bool error = false; - sval = DEFAULT_HOSTNAME; - if (prefs.putString(HOSTNAME_ENTRY, sval) == 0) - error = true; - bbuf = DEFAULT_NOTIFICATION_TYPE; - if (prefs.putChar(NOTIFICATION_TYPE, bbuf) == 0) - error = true; - sval = DEFAULT_TOKEN; - if (prefs.putString(NOTIFICATION_T1, sval) != sval.length()) - error = true; - if (prefs.putString(NOTIFICATION_T2, sval) != sval.length()) - error = true; - if (prefs.putString(NOTIFICATION_TS, sval) != sval.length()) - error = true; - sval = DEFAULT_STA_SSID; - if (prefs.putString(STA_SSID_ENTRY, sval) == 0) - error = true; - sval = DEFAULT_STA_PWD; - if (prefs.putString(STA_PWD_ENTRY, sval) != sval.length()) - error = true; - sval = DEFAULT_AP_SSID; - if (prefs.putString(AP_SSID_ENTRY, sval) == 0) - error = true; - sval = DEFAULT_AP_PWD; - if (prefs.putString(AP_PWD_ENTRY, sval) != sval.length()) - error = true; - bbuf = DEFAULT_AP_CHANNEL; - if (prefs.putChar(AP_CHANNEL_ENTRY, bbuf) == 0) - error = true; - bbuf = DEFAULT_STA_IP_MODE; - if (prefs.putChar(STA_IP_MODE_ENTRY, bbuf) == 0) - error = true; - bbuf = DEFAULT_HTTP_STATE; - if (prefs.putChar(HTTP_ENABLE_ENTRY, bbuf) == 0) - error = true; - bbuf = DEFAULT_TELNET_STATE; - if (prefs.putChar(TELNET_ENABLE_ENTRY, bbuf) == 0) - error = true; - bbuf = DEFAULT_RADIO_MODE; - if (prefs.putChar(ESP_RADIO_MODE, bbuf) == 0) - error = true; - ibuf = DEFAULT_WEBSERVER_PORT; - if (prefs.putUShort(HTTP_PORT_ENTRY, ibuf) == 0) - error = true; - ibuf = DEFAULT_TELNETSERVER_PORT; - if (prefs.putUShort(TELNET_PORT_ENTRY, ibuf) == 0) - error = true; - sval = DEFAULT_STA_IP; - if (prefs.putInt(STA_IP_ENTRY, wifi_config.IP_int_from_string(sval)) == 0) - error = true; - sval = DEFAULT_STA_GW; - if (prefs.putInt(STA_GW_ENTRY, wifi_config.IP_int_from_string(sval)) == 0) - error = true; - sval = DEFAULT_STA_MK; - if (prefs.putInt(STA_MK_ENTRY, wifi_config.IP_int_from_string(sval)) == 0) - error = true; - sval = DEFAULT_AP_IP; - if (prefs.putInt(AP_IP_ENTRY, wifi_config.IP_int_from_string(sval)) == 0) - error = true; - prefs.end(); + for (Setting *s = Setting::List; s; s = s->next()) { + if (s->getDescription()) { + s->setDefault(); + } + } + // TODO commit the changes and check that for errors if (error) grbl_send(CLIENT_ALL, "[MSG:WiFi reset error]\r\n"); - else - grbl_send(CLIENT_ALL, "[MSG:WiFi reset done]\r\n"); + grbl_send(CLIENT_ALL, "[MSG:WiFi reset done]\r\n"); } bool WiFiConfig::Is_WiFi_on() { return !(WiFi.getMode() == WIFI_MODE_NULL); diff --git a/Grbl_Esp32/wificonfig.h b/Grbl_Esp32/wificonfig.h index e0c69c35..011b62d0 100644 --- a/Grbl_Esp32/wificonfig.h +++ b/Grbl_Esp32/wificonfig.h @@ -19,25 +19,6 @@ */ //Preferences entries -#define HOSTNAME_ENTRY "ESP_HOSTNAME" -#define STA_SSID_ENTRY "STA_SSID" -#define STA_PWD_ENTRY "STA_PWD" -#define STA_IP_ENTRY "STA_IP" -#define STA_GW_ENTRY "STA_GW" -#define STA_MK_ENTRY "STA_MK" -#define AP_SSID_ENTRY "AP_SSID" -#define AP_PWD_ENTRY "AP_PWD" -#define AP_IP_ENTRY "AP_IP" -#define AP_CHANNEL_ENTRY "AP_CHANNEL" -#define HTTP_ENABLE_ENTRY "HTTP_ON" -#define HTTP_PORT_ENTRY "HTTP_PORT" -#define TELNET_ENABLE_ENTRY "TELNET_ON" -#define TELNET_PORT_ENTRY "TELNET_PORT" -#define STA_IP_MODE_ENTRY "STA_IP_MODE" -#define NOTIFICATION_TYPE "NOTIF_TYPE" -#define NOTIFICATION_T1 "NOTIF_T1" -#define NOTIFICATION_T2 "NOTIF_T2" -#define NOTIFICATION_TS "NOTIF_TS" //Notifications #define ESP_PUSHOVER_NOTIFICATION 1 diff --git a/Grbl_Esp32/wifiservices.cpp b/Grbl_Esp32/wifiservices.cpp index d094ad3d..fcc52d34 100644 --- a/Grbl_Esp32/wifiservices.cpp +++ b/Grbl_Esp32/wifiservices.cpp @@ -20,16 +20,13 @@ #ifdef ARDUINO_ARCH_ESP32 -#include "config.h" +#include "grbl.h" #ifdef ENABLE_WIFI #include #include #include -#include -#include "report.h" -#include "wificonfig.h" #include "wifiservices.h" #ifdef ENABLE_MDNS #include @@ -60,13 +57,8 @@ bool WiFiServices::begin() { bool no_error = true; //Sanity check if (WiFi.getMode() == WIFI_OFF) return false; - String h; - Preferences prefs; - //Get hostname - String defV = DEFAULT_HOSTNAME; - prefs.begin(NAMESPACE, true); - h = prefs.getString(HOSTNAME_ENTRY, defV); - prefs.end(); + String h = wifi_hostname->get(); + //Start SPIFFS SPIFFS.begin(true); #ifdef ENABLE_OTA @@ -148,8 +140,12 @@ void WiFiServices::handle() { COMMANDS::wait(0); //to avoid mixed mode due to scan network if (WiFi.getMode() == WIFI_AP_STA) { - if (WiFi.scanComplete() != WIFI_SCAN_RUNNING) + // In principle it should be sufficient to check for != WIFI_SCAN_RUNNING, + // but that does not work well. Doing so makes scans in AP mode unreliable. + // Sometimes the first try works, but subsequent scans fail. + if (WiFi.scanComplete() >= 0) { WiFi.enableSTA(false); + } } #ifdef ENABLE_OTA ArduinoOTA.handle(); diff --git a/build-all.py b/build-all.py index 45f53df8..61ddd443 100755 --- a/build-all.py +++ b/build-all.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python # Note: If you experience random errors running this script within # VSCode, try running it from a regular terminal window. Some VSCode diff --git a/build-machine.py b/build-machine.py index 29f75597..7a6cb1d3 100755 --- a/build-machine.py +++ b/build-machine.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python # Compile Grbl_ESP32 for each of the machines defined in Machines/ . # Add-on files are built on top of a single base. diff --git a/builder.py b/builder.py index 68f9d095..6ee0e1db 100755 --- a/builder.py +++ b/builder.py @@ -22,6 +22,7 @@ def buildMachine(baseName, verbose=True, extraArgs=None): else: app = subprocess.Popen(cmd, env=env, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, bufsize=1) for line in app.stdout: + line = line.decode('utf8') if "Took" in line or 'Uploading' in line or "error" in line.lower(): print(line, end='') app.wait() diff --git a/configure-features.py b/configure-features.py index e0b5e513..4419ae06 100755 --- a/configure-features.py +++ b/configure-features.py @@ -163,9 +163,9 @@ def changeConfig(src, enabled, disabled, verbose=False): if len(enabled) > 0: for name in enabled: s = enablePrefix + name - m = re.match(r'^s*//\s*#define\s+(' + s + r'.*)$', line) + m = re.match(r'^s*(//)*\s*#define\s+(' + s + r'.*)$', line) if m: - dstLine = "#define " + m.group(1) + '\n' + dstLine = "#define " + m.group(2) + '\n' numEnabledDic[name] += 1 break if len(disabled) > 0: diff --git a/debug.ini b/debug.ini new file mode 100644 index 00000000..0d148f7f --- /dev/null +++ b/debug.ini @@ -0,0 +1,30 @@ +; Edit this to optimize for your debug environment and +; uncomment the extra_configs line in platformio.ini + +[env] +; Windows +upload_port = COM3 +monitor_port = COM3 +; macOS +;upload_port = /dev/cu.usbserial-* +;monitor_port = /dev/cu.usbserial-* +; Linux +;upload_port = /dev/ttyUSB* +;monitor_port = /dev/ttyUSB* +build_flags = + ${common.build_flags} + -DMACHINE_FILENAME=test_drive.h + +[env:debug] +; You can switch between debugging tools using debug_tool option +; https://docs.platformio.org/en/latest/plus/debugging.html#tools-debug-probes +; +; Minimodule +;;debug_tool = minimodule +;;upload_protocol = minimodule +; ESP-Prog +debug_tool = esp-prog +upload_protocol = esp-prog + +;debug_init_break = thb app_main +debug_init_break = thb setup diff --git a/platformio.ini b/platformio.ini index e9ec47f0..db4c63cd 100644 --- a/platformio.ini +++ b/platformio.ini @@ -44,4 +44,3 @@ build_flags = src_filter = +<*.h> +<*.s> +<*.S> +<*.cpp> +<*.c> +<*.ino> + -<.git/> - - - - -