diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp index cefab842..39488722 100644 --- a/Grbl_Esp32/Custom/CoreXY.cpp +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -43,7 +43,6 @@ const float geometry_factor = 1.0; const float geometry_factor = 2.0; #endif -static float last_motors[MAX_N_AXIS] = { 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs static float last_cartesian[MAX_N_AXIS] = {}; // prototypes for helper functions @@ -51,9 +50,28 @@ float three_axis_dist(float* point1, float* point2); void machine_init() { // print a startup message to show the kinematics are enable + +#ifdef MIDTBOT + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY (midTbot) Kinematics Init"); +#else grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY Kinematics Init"); +#endif } +// Converts Cartesian to motors with no motion control +static void cartesian_to_motors(float* position) { + float motors[MAX_N_AXIS]; + + motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; + motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; + + position[X_AXIS] = motors[X_AXIS]; + position[Y_AXIS] = motors[Y_AXIS]; + + // Z and higher just pass through unchanged +} + +// Cycle mask is 0 unless the user sends a single axis command like $HZ // This will always return true to prevent the normal Grbl homing cycle bool user_defined_homing(uint8_t cycle_mask) { uint8_t n_cycle; // each home is a multi cycle operation approach, pulloff, approach..... @@ -61,15 +79,10 @@ bool user_defined_homing(uint8_t cycle_mask) { float max_travel; uint8_t axis; - // check for single axis homing - if (cycle_mask != 0) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY Single axis homing not allowed. Use $H only"); - return true; - } - // check for multi axis homing per cycle ($Homing/Cycle0=XY type)...not allowed in CoreXY bool setting_error = false; - for (int cycle = 0; cycle < 3; cycle++) { + auto n_axis = number_axis->get(); + for (int cycle = 0; cycle < n_axis; cycle++) { if (numberOfSetBits(homing_cycle[cycle]->get()) > 1) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, @@ -90,10 +103,22 @@ bool user_defined_homing(uint8_t cycle_mask) { pl_data->motion.systemMotion = 1; pl_data->motion.noFeedOverride = 1; - for (int cycle = 0; cycle < 3; cycle++) { - AxisMask mask = homing_cycle[cycle]->get(); + uint8_t cycle_count = (cycle_mask == 0) ? n_axis : 1; // if we have a cycle_mask, we are only going to do one axis + + AxisMask mask = 0; + for (int cycle = 0; cycle < cycle_count; cycle++) { + // if we have a cycle_mask, do that. Otherwise get the cycle from the settings + mask = cycle_mask ? cycle_mask : homing_cycle[cycle]->get(); + + // If not X or Y do a normal home + if (!(bitnum_istrue(mask, X_AXIS) || bitnum_istrue(mask, Y_AXIS))) { + limits_go_home(mask); // Homing cycle 0 + continue; // continue to next item in for loop + } + mask = motors_set_homing_mode(mask, true); // non standard homing motors will do their own thing and get removed from the mask - for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) { + + for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) { if (bit(axis) == mask) { // setup for the homing of this axis bool approach = true; @@ -117,10 +142,12 @@ bool user_defined_homing(uint8_t cycle_mask) { approach ? target[axis] = max_travel : target[axis] = -max_travel; } - target[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); + for (int axis = Z_AXIS; axis < n_axis; axis++) { + target[axis] = sys_position[axis] / axis_settings[axis]->steps_per_mm->get(); + } // convert back to motor steps - inverse_kinematics(target); + cartesian_to_motors(target); pl_data->feed_rate = homing_rate; // feed or seek rates plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion. @@ -190,7 +217,7 @@ bool user_defined_homing(uint8_t cycle_mask) { } while (n_cycle-- > 0); } } - } + } // for // after sussefully setting X & Y axes, we set the current positions @@ -205,10 +232,13 @@ bool user_defined_homing(uint8_t cycle_mask) { last_cartesian[X_AXIS] = target[X_AXIS]; last_cartesian[Y_AXIS] = target[Y_AXIS]; - last_cartesian[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); + + for (int axis = Z_AXIS; axis < n_axis; axis++) { + last_cartesian[axis] = sys_position[axis] / axis_settings[axis]->steps_per_mm->get(); + } // convert to motors - inverse_kinematics(target); + cartesian_to_motors(target); // convert to steps for (axis = X_AXIS; axis <= Y_AXIS; axis++) { sys_position[axis] = target[axis] * axis_settings[axis]->steps_per_mm->get(); @@ -224,29 +254,21 @@ bool user_defined_homing(uint8_t cycle_mask) { return true; } -// This function is used by Grbl convert Cartesian to motors -// this does not do any motion control -void inverse_kinematics(float* position) { - float motors[3]; +static void transform_cartesian_to_motors(float* motors, float* cartesian) { + motors[X_AXIS] = geometry_factor * cartesian[X_AXIS] + cartesian[Y_AXIS]; + motors[Y_AXIS] = geometry_factor * cartesian[X_AXIS] - cartesian[Y_AXIS]; - motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; - motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; - motors[Z_AXIS] = position[Z_AXIS]; - - position[0] = motors[0]; - position[1] = motors[1]; - position[2] = motors[2]; + auto n_axis = number_axis->get(); + for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) { + motors[axis] = cartesian[axis]; + } } // Inverse Kinematics calculates motor positions from real world cartesian positions -// position is the current position +// position is the old machine position, target the new machine position // Breaking into segments is not needed with CoreXY, because it is a linear system. -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos -{ +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { float dx, dy, dz; // distances in each cartesian axis - float motors[MAX_N_AXIS]; - - float feed_rate = pl_data->feed_rate; // save original feed rate // calculate cartesian move distance for each axis dx = target[X_AXIS] - position[X_AXIS]; @@ -254,31 +276,31 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio dz = target[Z_AXIS] - position[Z_AXIS]; float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz)); - motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS]; - motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_AXIS]; - motors[Z_AXIS] = target[Z_AXIS]; + auto n_axis = number_axis->get(); - float motor_distance = three_axis_dist(motors, last_motors); + float motors[n_axis]; + transform_cartesian_to_motors(motors, target); if (!pl_data->motion.rapidMotion) { - pl_data->feed_rate *= (motor_distance / dist); + float last_motors[n_axis]; + transform_cartesian_to_motors(last_motors, position); + pl_data->feed_rate *= (three_axis_dist(motors, last_motors) / dist); } - memcpy(last_motors, motors, sizeof(motors)); - - mc_line(motors, pl_data); + return mc_line(motors, pl_data); } // motors -> cartesian -void forward_kinematics(float* position) { - float calc_fwd[MAX_N_AXIS]; - +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + // apply the forward kinemetics to the machine coordinates // https://corexy.com/theory.html - calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]); - calc_fwd[Y_AXIS] = 0.5 * (position[X_AXIS] - position[Y_AXIS]); + //calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]); + cartesian[X_AXIS] = 0.5 * (motors[X_AXIS] + motors[Y_AXIS]) / geometry_factor; + cartesian[Y_AXIS] = 0.5 * (motors[X_AXIS] - motors[Y_AXIS]); - position[X_AXIS] = calc_fwd[X_AXIS]; - position[Y_AXIS] = calc_fwd[Y_AXIS]; + for (int axis = Z_AXIS; axis < n_axis; axis++) { + cartesian[axis] = motors[axis]; + } } bool kinematics_pre_homing(uint8_t cycle_mask) { @@ -286,14 +308,13 @@ bool kinematics_pre_homing(uint8_t cycle_mask) { } void kinematics_post_homing() { - gc_state.position[X_AXIS] = last_cartesian[X_AXIS]; - gc_state.position[Y_AXIS] = last_cartesian[Y_AXIS]; - gc_state.position[Z_AXIS] = last_cartesian[Z_AXIS]; + auto n_axis = number_axis->get(); + memcpy(gc_state.position, last_cartesian, n_axis * sizeof(last_cartesian[0])); } -// this is used used by Grbl soft limits to see if the range of the machine is exceeded. -uint8_t kinematic_limits_check(float* target) { - return true; +// this is used used by Limits.cpp to see if the range of the machine is exceeded. +bool limitsCheckTravel(float* target) { + return false; } void user_m30() {} diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp index 01ddaa50..086e28fc 100644 --- a/Grbl_Esp32/Custom/atari_1020.cpp +++ b/Grbl_Esp32/Custom/atari_1020.cpp @@ -58,7 +58,8 @@ void machine_init() { NULL, // parameters 1, // priority &solenoidSyncTaskHandle, - 0 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); // setup a task that will do the custom homing sequence xTaskCreatePinnedToCore(atari_home_task, // task @@ -67,22 +68,20 @@ void machine_init() { NULL, // parameters 1, // priority &atariHomingTaskHandle, - 0 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); } // this task tracks the Z position and sets the solenoid void solenoidSyncTask(void* pvParameters) { int32_t current_position[N_AXIS]; // copy of current location - float m_pos[N_AXIS]; // machine position in mm TickType_t xLastWakeTime; const TickType_t xSolenoidFrequency = SOLENOID_TASK_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 - memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step - system_convert_array_steps_to_mpos(m_pos, current_position); // convert to millimeters - calc_solenoid(m_pos[Z_AXIS]); // calculate kinematics and move the servos + calc_solenoid(system_get_mpos()[Z_AXIS]); // calculate kinematics and move the servos vTaskDelayUntil(&xLastWakeTime, xSolenoidFrequency); } } diff --git a/Grbl_Esp32/Custom/custom_code_template.cpp b/Grbl_Esp32/Custom/custom_code_template.cpp index bd468f6d..54d5f6ec 100644 --- a/Grbl_Esp32/Custom/custom_code_template.cpp +++ b/Grbl_Esp32/Custom/custom_code_template.cpp @@ -24,37 +24,39 @@ ======================================================================= 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. +configured to call some optional functions. These functions have weak definitions +in the main code. If you create your own version they will be used instead -To use this file, copy it to a name of your own choosing, and also copy -Machines/template.h to a similar name. +Put all of your functions in a .cpp file in the "Custom" folder. +Add this to your machine definition file +#define CUSTOM_CODE_FILENAME "../Custom/YourFile.cpp" -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 +Be careful to return the correct values =============================================================================== +Below are all the current weak function + */ -#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. +This function is used as a one time setup for your machine. */ void machine_init() {} -#endif -#ifdef USE_CUSTOM_HOMING +/* +This is used to initialize a display. +*/ +void display_init() {} + +/* + limitsCheckTravel() is called to check soft limits + It returns true if the motion is outside the limit values +*/ +bool limitsCheckTravel() { + return false; +} + /* user_defined_homing(uint8_t cycle_mask) is called at the begining of the normal Grbl_ESP32 homing sequence. If user_defined_homing(uint8_t cycle_mask) returns false, the rest of normal Grbl_ESP32 @@ -66,17 +68,14 @@ bool user_defined_homing(uint8_t cycle_mask) { // True = done with homing, false = continue with normal Grbl_ESP32 homing return true; } -#endif -#ifdef USE_KINEMATICS /* Inverse Kinematics converts X,Y,Z cartesian coordinate to the steps on your "joint" motors. It requires the following three functions: */ /* - inverse_kinematics() looks at incoming move commands and modifies - them before Grbl_ESP32 puts them in the motion planner. + cartesian_to_motors() converts from cartesian coordinates to motor space. 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 @@ -86,9 +85,9 @@ bool user_defined_homing(uint8_t cycle_mask) { 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) { +bool cartesian_to_motors(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); + return mc_line(target, pl_data); } /* @@ -97,8 +96,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio cycle_mask is a bit mask of the axes being homed this time. */ -bool kinematics_pre_homing(uint8_t cycle_mask)) -{ +bool kinematics_pre_homing(uint8_t cycle_mask) { return false; // finish normal homing cycle } @@ -106,51 +104,34 @@ bool kinematics_pre_homing(uint8_t cycle_mask)) kinematics_post_homing() is called at the end of normal homing */ void kinematics_post_homing() {} -#endif -#ifdef USE_FWD_KINEMATICS /* - The status command uses forward_kinematics() to convert + The status command uses motors_to_cartesian() 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) { +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { // 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 diff --git a/Grbl_Esp32/Custom/oled_basic.cpp b/Grbl_Esp32/Custom/oled_basic.cpp new file mode 100644 index 00000000..b1e2862b --- /dev/null +++ b/Grbl_Esp32/Custom/oled_basic.cpp @@ -0,0 +1,283 @@ +/* + oled_basic.cpp + Part of Grbl_ESP32 + + copyright (c) 2018 - Bart Dring This file was modified for use on the ESP32 + CPU. Do not use this with Grbl for atMega328P + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . + + -------------------------------------------------------------- + + This is a minimal implentation of a display as a test project. + It is designed to be used with a machine that has no easily accessible serial connection + It shows basic status and connection information. + + When in alarm mode it will show the current Wifi/BT paramaters and status + Most machines will start in alarm mode (needs homing) + If the machine is running a job from SD it will show the progress + In other modes it will show state and 3 axis DROs + Thats All! + + Library Infor: + https://github.com/ThingPulse/esp8266-oled-ssd1306 + + Install to PlatformIO with this typed at the terminal + platformio lib install 562 + + Add this to your machine definition file + #define DISPLAY_CODE_FILENAME "Custom/oled_basic.cpp" +*/ + +// Include the correct display library + +#include "SSD1306Wire.h" +#include "../src/WebUI/WebSettings.h" + +#ifndef OLED_ADDRESS +# define OLED_ADDRESS 0x3c +#endif + +#ifndef OLED_SDA +# define OLED_SDA GPIO_NUM_14 +#endif + +#ifndef OLED_SCL +# define OLED_SCL GPIO_NUM_13 +#endif + +#ifndef OLED_GEOMETRY +# define OLED_GEOMETRY GEOMETRY_128_64 +#endif + +SSD1306Wire display(OLED_ADDRESS, OLED_SDA, OLED_SCL, OLED_GEOMETRY); + +static TaskHandle_t displayUpdateTaskHandle = 0; + +// This displays the status of the ESP32 Radios...BT, WiFi, etc +void displayRadioInfo() { + String radio_addr = ""; + String radio_name = ""; + String radio_status = ""; + +#ifdef ENABLE_BLUETOOTH + if (WebUI::wifi_radio_mode->get() == ESP_BT) { + radio_name = String("BT: ") + WebUI::bt_name->get(); + } +#endif +#ifdef ENABLE_WIFI + if ((WiFi.getMode() == WIFI_MODE_STA) || (WiFi.getMode() == WIFI_MODE_APSTA)) { + radio_name = "STA: " + WiFi.SSID(); + radio_addr = WiFi.localIP().toString(); + } else if ((WiFi.getMode() == WIFI_MODE_AP) || (WiFi.getMode() == WIFI_MODE_APSTA)) { + radio_name = String("AP:") + WebUI::wifi_ap_ssid->get(); + radio_addr = WiFi.softAPIP().toString(); + } +#endif + +#ifdef WIFI_OR_BLUETOOTH + if (WebUI::wifi_radio_mode->get() == ESP_RADIO_OFF) { + radio_name = "Radio Mode: None"; + } +#else + radio_name = "Radio Mode:Disabled"; +#endif + + display.setTextAlignment(TEXT_ALIGN_LEFT); + display.setFont(ArialMT_Plain_10); + + if (sys.state == State::Alarm) { // print below Alarm: + display.drawString(0, 18, radio_name); + display.drawString(0, 30, radio_addr); + + } else { // print next to status + if (WebUI::wifi_radio_mode->get() == ESP_BT) { + display.drawString(55, 2, radio_name); + } else { + display.drawString(55, 2, radio_addr); + } + } +} +// Here changes begin Here changes begin Here changes begin Here changes begin Here changes begin + +void draw_checkbox(int16_t x, int16_t y, int16_t width, int16_t height, bool checked) { + if (checked) + display.fillRect(x, y, width, height); // If log.0 + else + display.drawRect(x, y, width, height); // If log.1 +} + +void displayDRO() { + uint8_t oled_y_pos; + //float wco[MAX_N_AXIS]; + + display.setTextAlignment(TEXT_ALIGN_LEFT); + display.setFont(ArialMT_Plain_10); + + char axisVal[20]; + + display.drawString(80, 14, "L"); // Limit switch + + auto n_axis = number_axis->get(); + AxisMask lim_pin_state = limits_get_state(); + ControlPins ctrl_pin_state = system_control_get_state(); + bool prb_pin_state = probe_get_state(); + + display.setTextAlignment(TEXT_ALIGN_RIGHT); + + float* print_position = system_get_mpos(); + if (bit_istrue(status_mask->get(), RtStatus::Position)) { + display.drawString(60, 14, "M Pos"); + } else { + display.drawString(60, 14, "W Pos"); + mpos_to_wpos(print_position); + } + + for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { + oled_y_pos = 24 + (axis * 10); + + String axis_letter = String(report_get_axis_letter(axis)); + axis_letter += ":"; + display.setTextAlignment(TEXT_ALIGN_LEFT); + display.drawString(0, oled_y_pos, axis_letter); // String('X') + ":"); + + display.setTextAlignment(TEXT_ALIGN_RIGHT); + snprintf(axisVal, 20 - 1, "%.3f", print_position[axis]); + display.drawString(60, oled_y_pos, axisVal); + + if (limitsSwitchDefined(axis, 0)) { // olny draw the box if a switch has been defined + draw_checkbox(80, 27 + (axis * 10), 7, 7, bit_istrue(lim_pin_state, bit(axis))); + } + } + + oled_y_pos = 14; + + if (PROBE_PIN != UNDEFINED_PIN) { + display.drawString(110, oled_y_pos, "P"); + draw_checkbox(120, oled_y_pos + 3, 7, 7, prb_pin_state); + oled_y_pos += 10; + } + +#ifdef CONTROL_FEED_HOLD_PIN + display.drawString(110, oled_y_pos, "H"); + draw_checkbox(120, oled_y_pos + 3, 7, 7, ctrl_pin_state.bit.feedHold); + oled_y_pos += 10; +#endif + +#ifdef CONTROL_CYCLE_START_PIN + display.drawString(110, oled_y_pos, "S"); + draw_checkbox(120, oled_y_pos + 3, 7, 7, ctrl_pin_state.bit.cycleStart); + oled_y_pos += 10; +#endif + +#ifdef CONTROL_RESET_PIN + display.drawString(110, oled_y_pos, "R"); + draw_checkbox(120, oled_y_pos + 3, 7, 7, ctrl_pin_state.bit.reset); + oled_y_pos += 10; +#endif + +#ifdef CONTROL_SAFETY_DOOR_PIN + display.drawString(110, oled_y_pos, "D"); + draw_checkbox(120, oled_y_pos + 3, 7, 7, ctrl_pin_state.bit.safetyDoor); +#endif +} + +void displayUpdate(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xDisplayFrequency = 100; // in ticks (typically ms) + xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. + + vTaskDelay(2500); + uint16_t sd_file_ticker = 0; + + display.init(); + display.flipScreenVertically(); + + while (true) { + display.clear(); + + String state_string = ""; + + display.setTextAlignment(TEXT_ALIGN_LEFT); + display.setFont(ArialMT_Plain_16); + display.drawString(0, 0, report_state_text()); + + if (get_sd_state(false) == SDState::BusyPrinting) { + display.clear(); + display.setTextAlignment(TEXT_ALIGN_CENTER); + display.setFont(ArialMT_Plain_10); + state_string = "SD File"; + for (int i = 0; i < sd_file_ticker % 10; i++) { + state_string += "."; + } + sd_file_ticker++; + display.drawString(63, 0, state_string); + + char path[50]; + sd_get_current_filename(path); + display.drawString(63, 12, path); + + int progress = sd_report_perc_complete(); + // draw the progress bar + display.drawProgressBar(0, 45, 120, 10, progress); + + // draw the percentage as String + display.setFont(ArialMT_Plain_10); + display.setTextAlignment(TEXT_ALIGN_CENTER); + display.drawString(64, 25, String(progress) + "%"); + + } else if (sys.state == State::Alarm) { + displayRadioInfo(); + } else { + displayDRO(); + displayRadioInfo(); + } + + display.display(); + + vTaskDelayUntil(&xLastWakeTime, xDisplayFrequency); + } +} + +void display_init() { + // Initialising the UI will init the display too. + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Init Basic OLED SDA:%s SCL:%s", pinName(OLED_SDA), pinName(OLED_SCL)); + display.init(); + + display.flipScreenVertically(); + + display.clear(); + + display.setTextAlignment(TEXT_ALIGN_CENTER); + display.setFont(ArialMT_Plain_10); + + String mach_name = MACHINE_NAME; + // remove characters from the end until the string fits + while (display.getStringWidth(mach_name) > 128) { + mach_name = mach_name.substring(0, mach_name.length() - 1); + } + display.drawString(63, 0, mach_name); + + display.display(); + + xTaskCreatePinnedToCore(displayUpdate, // task + "displayUpdateTask", // name for task + 4096, // size of task stack + NULL, // parameters + 1, // priority + &displayUpdateTaskHandle, + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core + ); +} diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp index a82de59b..23b28a81 100644 --- a/Grbl_Esp32/Custom/parallel_delta.cpp +++ b/Grbl_Esp32/Custom/parallel_delta.cpp @@ -91,7 +91,6 @@ static float last_cartesian[N_AXIS] = { }; // A place to save the previous motor angles for distance/feed rate calcs // Z offset of the effector from the arm centers // prototypes for helper functions -int calc_forward_kinematics(float* angles, float* cartesian); KinematicError delta_calcInverse(float* cartesian, float* angles); KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta); float three_axis_dist(float* point1, float* point2); @@ -108,11 +107,9 @@ void machine_init() { delta_crank_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/CrankSideLength", LENGTH_FIXED_SIDE, 20.0, 500.0); delta_effector_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/EffectorSideLength", LENGTH_EFF_SIDE, 20.0, 500.0); - read_settings(); - // Calculate the Z offset at the arm zero angles ... // Z offset is the z distance from the motor axes to the end effector axes at zero angle - calc_forward_kinematics(angles, cartesian); // Sets the cartesian values + motors_to_cartesian(cartesian, angles, 3); // Sets the cartesian values // print a startup message to show the kinematics are enabled. Print the offset for reference grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]); @@ -126,32 +123,20 @@ void machine_init() { // DXL_COUNT_MAX, // DXL_COUNT_PER_RADIAN); } -bool user_defined_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing -#ifdef USE_CUSTOM_HOMING - return true; -#else - return false; -#endif -} -// This function is used by Grbl -void inverse_kinematics(float* position) { - float motor_angles[3]; +// bool user_defined_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing +// #ifdef USE_CUSTOM_HOMING +// return true; +// #else +// return false; +// #endif +// } - read_settings(); - delta_calcInverse(position, motor_angles); - position[0] = motor_angles[0]; - position[1] = motor_angles[1]; - position[2] = motor_angles[2]; -} - -// This function is used by Grbl -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos -{ +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { float dx, dy, dz; // distances in each cartesian axis float motor_angles[3]; - float seg_target[3]; // The target of the current segment + float seg_target[3]; // The target of the current segment float feed_rate = pl_data->feed_rate; // save original feed rate bool show_error = true; // shows error once @@ -162,16 +147,17 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start %3.3f %3.3f %3.3f", position[0], position[1], position[2]); // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target %3.3f %3.3f %3.3f", target[0], target[1], target[2]); - status = delta_calcInverse(position, motor_angles); + status = delta_calcInverse(position, last_angle); if (status == KinematicError::OUT_OF_RANGE) { //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error %3.3f %3.3f %3.3f", position[0], position[1], position[2]); - //start_position_error = true; + return false; } // Check the destination to see if it is in work area status = delta_calcInverse(target, motor_angles); if (status == KinematicError::OUT_OF_RANGE) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable error %3.3f %3.3f %3.3f", target[0], target[1], target[2]); + return false; } position[X_AXIS] += gc_state.coord_offset[X_AXIS]; @@ -198,21 +184,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // calculate the delta motor angles KinematicError status = delta_calcInverse(seg_target, motor_angles); - if (status == KinematicError ::NONE) { - float delta_distance = three_axis_dist(motor_angles, last_angle); - - // save angles for next distance calc - memcpy(last_angle, motor_angles, sizeof(motor_angles)); - - if (pl_data->motion.rapidMotion) { - pl_data->feed_rate = feed_rate; - } else { - pl_data->feed_rate = (feed_rate * delta_distance / segment_dist); - } - - mc_line(motor_angles, pl_data); - - } else { + if (status != KinematicError ::NONE) { if (show_error) { // grbl_msg_sendf(CLIENT_SERIAL, // MsgLevel::Info, @@ -223,35 +195,52 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // motor_angles[2]); show_error = false; } + return false; } + if (pl_data->motion.rapidMotion) { + pl_data->feed_rate = feed_rate; + } else { + float delta_distance = three_axis_dist(motor_angles, last_angle); + pl_data->feed_rate = (feed_rate * delta_distance / segment_dist); + } + + // mc_line() returns false if a jog is cancelled. + // In that case we stop sending segments to the planner. + if (!mc_line(motor_angles, pl_data)) { + return false; + } + + // save angles for next distance calc + // This is after mc_line() so that we do not update + // last_angle if the segment was discarded. + memcpy(last_angle, motor_angles, sizeof(motor_angles)); } + return true; } // this is used used by Grbl soft limits to see if the range of the machine is exceeded. -uint8_t kinematic_limits_check(float* target) { +bool limitsCheckTravel(float* target) { float motor_angles[3]; read_settings(); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin Soft Check %3.3f, %3.3f, %3.3f", target[0], target[1], target[2]); - KinematicError status = delta_calcInverse(target, motor_angles); - - switch (status) { + switch (delta_calcInverse(target, motor_angles)) { case KinematicError::OUT_OF_RANGE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target out of range"); - break; + return true; case KinematicError::ANGLE_TOO_NEGATIVE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max negative"); - break; + return true; case KinematicError::ANGLE_TOO_POSITIVE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max positive"); - break; + return true; case KinematicError::NONE: - break; + return false; } - return (status == KinematicError::NONE); + return false; } // inverse kinematics: cartesian -> angles @@ -286,19 +275,21 @@ KinematicError delta_calcInverse(float* cartesian, float* angles) { } // inverse kinematics: angles -> cartesian -int calc_forward_kinematics(float* angles, float* catesian) { +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + read_settings(); + float t = (f - e) * tan30 / 2; - float y1 = -(t + rf * cos(angles[0])); - float z1 = -rf * sin(angles[0]); + float y1 = -(t + rf * cos(motors[0])); + float z1 = -rf * sin(motors[0]); - float y2 = (t + rf * cos(angles[1])) * sin30; + float y2 = (t + rf * cos(motors[1])) * sin30; float x2 = y2 * tan60; - float z2 = -rf * sin(angles[1]); + float z2 = -rf * sin(motors[1]); - float y3 = (t + rf * cos(angles[2])) * sin30; + float y3 = (t + rf * cos(motors[2])) * sin30; float x3 = -y3 * tan60; - float z3 = -rf * sin(angles[2]); + float z3 = -rf * sin(motors[2]); float dnm = (y2 - y1) * x3 - (y3 - y1) * x2; @@ -321,14 +312,16 @@ int calc_forward_kinematics(float* angles, float* catesian) { // discriminant float d = b * b - (float)4.0 * a * c; - if (d < 0) - return -1; // non-existing point + if (d < 0) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "MSG:Fwd Kin Error"); + return; + } - catesian[Z_AXIS] = -(float)0.5 * (b + sqrt(d)) / a; - catesian[X_AXIS] = (a1 * catesian[Z_AXIS] + b1) / dnm; - catesian[Y_AXIS] = (a2 * catesian[Z_AXIS] + b2) / dnm; - return 0; + cartesian[Z_AXIS] = -(float)0.5 * (b + sqrt(d)) / a; + cartesian[X_AXIS] = (a1 * cartesian[Z_AXIS] + b1) / dnm; + cartesian[Y_AXIS] = (a2 * cartesian[Z_AXIS] + b2) / dnm; } + // helper functions, calculates angle theta1 (for YZ-pane) KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta) { float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 @@ -361,43 +354,15 @@ float three_axis_dist(float* point1, float* point2) { return sqrt(((point1[0] - point2[0]) * (point1[0] - point2[0])) + ((point1[1] - point2[1]) * (point1[1] - point2[1])) + ((point1[2] - point2[2]) * (point1[2] - point2[2]))); } -// called by reporting for WPos status -void forward_kinematics(float* position) { - float calc_fwd[N_AXIS]; - int status; - read_settings(); - - // convert the system position in steps to radians - float position_radians[N_AXIS]; - int32_t position_steps[N_AXIS]; // Copy current state of the system position variable - memcpy(position_steps, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(position_radians, position_steps); - - // grbl_msg_sendf( - // CLIENT_SERIAL, MsgLevel::Info, "Fwd Kin Angs %1.3f, %1.3f, %1.3f ", position_radians[0], position_radians[1], position_radians[2]); - - // detmine the position of the end effector joint center. - status = calc_forward_kinematics(position_radians, calc_fwd); - - if (status == 0) { - // apply offsets and set the returned values - position[X_AXIS] = calc_fwd[X_AXIS] - gc_state.coord_system[X_AXIS] + gc_state.coord_offset[X_AXIS]; - position[Y_AXIS] = calc_fwd[Y_AXIS] - gc_state.coord_system[Y_AXIS] + gc_state.coord_offset[Y_AXIS]; - position[Z_AXIS] = calc_fwd[Z_AXIS] - gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]; - } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "MSG:Fwd Kin Error"); - } -} - -bool kinematics_pre_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing -#ifdef USE_CUSTOM_HOMING - return true; -#else - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "kinematics_pre_homing"); - return false; -#endif -} +// bool kinematics_pre_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing +// #ifdef USE_CUSTOM_HOMING +// return true; +// #else +// //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "kinematics_pre_homing"); +// return false; +// #endif +// } void kinematics_post_homing() { #ifdef USE_CUSTOM_HOMING @@ -407,9 +372,7 @@ void kinematics_post_homing() { last_angle[Y_AXIS] = sys_position[Y_AXIS] / axis_settings[Y_AXIS]->steps_per_mm->get(); last_angle[Z_AXIS] = sys_position[Z_AXIS] / axis_settings[Z_AXIS]->steps_per_mm->get(); - read_settings(); - - calc_forward_kinematics(last_angle, last_cartesian); + motors_to_cartesian(last_cartesian, last_angle, 3); // grbl_msg_sendf(CLIENT_SERIAL, // MsgLevel::Info, diff --git a/Grbl_Esp32/Custom/polar_coaster.cpp b/Grbl_Esp32/Custom/polar_coaster.cpp index 85cf3f8a..7041fbb0 100644 --- a/Grbl_Esp32/Custom/polar_coaster.cpp +++ b/Grbl_Esp32/Custom/polar_coaster.cpp @@ -85,9 +85,8 @@ void kinematics_post_homing() { */ -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) { - //static float last_angle = 0; - //static float last_radius = 0; + +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { float dx, dy, dz; // distances in each cartesian axis float p_dx, p_dy, p_dz; // distances in each polar axis float dist, polar_dist; // the distances in both systems...used to determine feed rate @@ -139,11 +138,19 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // end determining new feed rate polar[RADIUS_AXIS] += x_offset; polar[Z_AXIS] += z_offset; + + // mc_line() returns false if a jog is cancelled. + // In that case we stop sending segments to the planner. + if (!mc_line(polar, pl_data)) { + return false; + } + + // last_radius = polar[RADIUS_AXIS]; last_angle = polar[POLAR_AXIS]; - mc_line(polar, pl_data); } // TO DO don't need a feedrate for rapids + return true; } /* @@ -159,18 +166,10 @@ position = the current machine position converted = position with forward kinematics applied. */ -void forward_kinematics(float* position) { - float original_position[N_AXIS]; // temporary storage of original - float print_position[N_AXIS]; - int32_t current_position[N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - original_position[X_AXIS] = print_position[X_AXIS] - gc_state.coord_system[X_AXIS] + gc_state.coord_offset[X_AXIS]; - original_position[Y_AXIS] = print_position[Y_AXIS] - gc_state.coord_system[Y_AXIS] + gc_state.coord_offset[Y_AXIS]; - original_position[Z_AXIS] = print_position[Z_AXIS] - gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]; - position[X_AXIS] = cos(radians(original_position[Y_AXIS])) * original_position[X_AXIS] * -1; - position[Y_AXIS] = sin(radians(original_position[Y_AXIS])) * original_position[X_AXIS]; - position[Z_AXIS] = original_position[Z_AXIS]; // unchanged +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + cartesian[X_AXIS] = cos(radians(motors[Y_AXIS])) * motors[X_AXIS] * -1; + cartesian[Y_AXIS] = sin(radians(motors[Y_AXIS])) * motors[X_AXIS]; + cartesian[Z_AXIS] = motors[Z_AXIS]; // unchanged } // helper functions diff --git a/Grbl_Esp32/src/data/favicon.ico b/Grbl_Esp32/data/favicon.ico similarity index 100% rename from Grbl_Esp32/src/data/favicon.ico rename to Grbl_Esp32/data/favicon.ico diff --git a/Grbl_Esp32/src/data/index.html.gz b/Grbl_Esp32/data/index.html.gz similarity index 100% rename from Grbl_Esp32/src/data/index.html.gz rename to Grbl_Esp32/data/index.html.gz diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index 50b014f8..ffba4148 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -47,14 +47,16 @@ Some features should not be changed. See notes below. // that the machine file might choose to undefine. // Note: HOMING_CYCLES are now settings +#define SUPPORT_TASK_CORE 1 // Reference: CONFIG_ARDUINO_RUNNING_CORE = 1 // Inverts pin logic of the control command pins based on a mask. This essentially means you can use // normally-closed switches on the specified pins, rather than the default normally-open switches. -// The mask order is Cycle Start | Feed Hold | Reset | Safety Door +// The mask order is ... +// Macro3 | Macro2 | Macro 1| Macr0 |Cycle Start | Feed Hold | Reset | Safety Door // For example B1101 will invert the function of the Reset pin. -#define INVERT_CONTROL_PIN_MASK B1111 +#define INVERT_CONTROL_PIN_MASK B00001111 -#define ENABLE_CONTROL_SW_DEBOUNCE // Default disabled. Uncomment to enable. +// #define ENABLE_CONTROL_SW_DEBOUNCE // Default disabled. Uncomment to enable. #define CONTROL_SW_DEBOUNCE_PERIOD 32 // in milliseconds default 32 microseconds #define USE_RMT_STEPS @@ -85,8 +87,6 @@ const int MAX_N_AXIS = 6; # define LIMIT_MASK B0 #endif -#define GRBL_MSG_LEVEL MsgLevel::Info // what level of [MSG:....] do you want to see 0=all off - // Serial baud rate // OK to change, but the ESP32 boot text is 115200, so you will not see that is your // serial monitor, sender, etc uses a different value than 115200 @@ -128,7 +128,7 @@ const int MAX_N_AXIS = 6; // "in the clear" over unsecured channels. It should be treated as a // "friendly suggestion" to prevent unwitting dangerous actions, rather than // as effective security against malice. -//#define ENABLE_AUTHENTICATION +// #define ENABLE_AUTHENTICATION //CONFIGURE_EYECATCH_END (DO NOT MODIFY THIS LINE) #ifdef ENABLE_AUTHENTICATION @@ -258,11 +258,6 @@ static const uint8_t NHomingLocateCycle = 1; // Integer (1-128) // previous tool path, as if nothing happened. #define ENABLE_SAFETY_DOOR_INPUT_PIN // ESP32 Leave this enabled for now .. code for undefined not ready -// After the safety door switch has been toggled and restored, this setting sets the power-up delay -// between restoring the spindle and coolant and resuming the cycle. -const double SAFETY_DOOR_SPINDLE_DELAY = 4.0; // Float (seconds) -const double SAFETY_DOOR_COOLANT_DELAY = 1.0; // Float (seconds) - // Inverts select limit pin states based on the following mask. This effects all limit pin functions, // such as hard limits and homing. However, this is different from overall invert limits setting. // This build option will invert only the limit pins defined here, and then the invert limits setting @@ -451,17 +446,7 @@ const int DWELL_TIME_STEP = 50; // Integer (1-255) (milliseconds) // While this is experimental, it is intended to be the future default method after testing //#define USE_RMT_STEPS -// Creates a delay between the direction pin setting and corresponding step pulse by creating -// another interrupt (Timer2 compare) to manage it. The main Grbl interrupt (Timer1 compare) -// sets the direction pins, and does not immediately set the stepper pins, as it would in -// normal operation. The Timer2 compare fires next to set the stepper pins after the step -// pulse delay time, and Timer2 overflow will complete the step pulse, except now delayed -// by the step pulse time plus the step pulse delay. (Thanks langwadt for the idea!) -// NOTE: Uncomment to enable. The recommended delay must be > 3us, and, when added with the -// user-supplied step pulse time, the total time must not exceed 127us. Reported successful -// values for certain setups have ranged from 5 to 20us. -// must use #define USE_RMT_STEPS for this to work -//#define STEP_PULSE_DELAY 10 // Step pulse delay in microseconds. Default disabled. +// STEP_PULSE_DELAY is now a setting...$Stepper/Direction/Delay // The number of linear motions in the planner buffer to be planned at any give time. The vast // majority of RAM that Grbl uses is based on this buffer size. Only increase if there is extra @@ -582,8 +567,8 @@ const int DEBOUNCE_PERIOD = 32; // in milliseconds default 32 microseconds // Configure options for the parking motion, if enabled. #define PARKING_AXIS Z_AXIS // Define which axis that performs the parking motion const double PARKING_TARGET = -5.0; // Parking axis target. In mm, as machine coordinate. -const double PARKING_RATE = 500.0; // Parking fast rate after pull-out in mm/min. -const double PARKING_PULLOUT_RATE = 100.0; // Pull-out/plunge slow feed rate in mm/min. +const double PARKING_RATE = 800.0; // Parking fast rate after pull-out in mm/min. +const double PARKING_PULLOUT_RATE = 250.0; // Pull-out/plunge slow feed rate in mm/min. const double PARKING_PULLOUT_INCREMENT = 5.0; // Spindle pull-out and plunge distance in mm. Incremental distance. // Must be positive value or equal to zero. diff --git a/Grbl_Esp32/src/CustomCode.cpp b/Grbl_Esp32/src/CustomCode.cpp index 7ee3f88a..f0e0e626 100644 --- a/Grbl_Esp32/src/CustomCode.cpp +++ b/Grbl_Esp32/src/CustomCode.cpp @@ -6,3 +6,7 @@ #ifdef CUSTOM_CODE_FILENAME # include CUSTOM_CODE_FILENAME #endif + +#ifdef DISPLAY_CODE_FILENAME +# include DISPLAY_CODE_FILENAME +#endif \ No newline at end of file diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 6ca3728c..9c5ac586 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -42,6 +42,14 @@ # define DEFAULT_STEP_PULSE_MICROSECONDS 3 // $0 #endif +#ifndef DEFAULT_STEP_ENABLE_DELAY +# define DEFAULT_STEP_ENABLE_DELAY 0 +#endif + +#ifndef STEP_PULSE_DELAY +# define STEP_PULSE_DELAY 0 +#endif + #ifndef DEFAULT_STEPPER_IDLE_LOCK_TIME # define DEFAULT_STEPPER_IDLE_LOCK_TIME 250 // $1 msec (0-254, 255 keeps steppers enabled) #endif @@ -159,6 +167,10 @@ # define DEFAULT_LASER_MODE 0 // false #endif +#ifndef DEFAULT_LASER_FULL_POWER +# define DEFAULT_LASER_FULL_POWER 1000 +#endif + #ifndef DEFAULT_SPINDLE_RPM_MAX // $30 # define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm #endif @@ -183,6 +195,10 @@ # define DEFAULT_SPINDLE_DELAY_SPINUP 0 #endif +#ifndef DEFAULT_COOLANT_DELAY_TURNON +# define DEFAULT_COOLANT_DELAY_TURNON 1.0 +#endif + #ifndef DEFAULT_SPINDLE_DELAY_SPINDOWN # define DEFAULT_SPINDLE_DELAY_SPINDOWN 0 #endif @@ -457,6 +473,10 @@ // This can eliminate checking to see if the pin is defined because // the overridden pinMode and digitalWrite functions will deal with it. +#ifndef SDCARD_DET_PIN +# define SDCARD_DET_PIN UNDEFINED_PIN +#endif + #ifndef STEPPERS_DISABLE_PIN # define STEPPERS_DISABLE_PIN UNDEFINED_PIN #endif @@ -643,3 +663,19 @@ #ifndef USER_ANALOG_PIN_3_FREQ # define USER_ANALOG_PIN_3_FREQ 5000 #endif + +#ifndef DEFAULT_USER_MACRO0 +# define DEFAULT_USER_MACRO0 "" +#endif + +#ifndef DEFAULT_USER_MACRO1 +# define DEFAULT_USER_MACRO1 "" +#endif + +#ifndef DEFAULT_USER_MACRO2 +# define DEFAULT_USER_MACRO2 "" +#endif + +#ifndef DEFAULT_USER_MACRO3 +# define DEFAULT_USER_MACRO3 "" +#endif diff --git a/Grbl_Esp32/src/Eeprom.cpp b/Grbl_Esp32/src/Eeprom.cpp deleted file mode 100644 index 048524a5..00000000 --- a/Grbl_Esp32/src/Eeprom.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/* - Eeprom.cpp - Coordinate data stored in EEPROM - Part of Grbl - Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC - - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P - - Grbl is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#include "Grbl.h" -#include "Eeprom.h" - -void memcpy_to_eeprom_with_checksum(unsigned int destination, const char* source, unsigned int size) { - unsigned char checksum = 0; - for (; size > 0; size--) { - unsigned char data = static_cast(*source++); - // Note: This checksum calculation is broken as described below. - checksum = (checksum << 1) || (checksum >> 7); - checksum += data; - EEPROM.write(destination++, *(source++)); - } - EEPROM.write(destination, checksum); - EEPROM.commit(); -} - -int memcpy_from_eeprom_with_old_checksum(char* destination, unsigned int source, unsigned int size) { - unsigned char data, checksum = 0; - for (; size > 0; size--) { - data = EEPROM.read(source++); - // Note: This checksum calculation is broken - the || should be just | - - // thus making the checksum very weak. - // We leave it as-is so we can read old data after a firmware upgrade. - // The new storage format uses the tagged NVS mechanism, avoiding this bug. - checksum = (checksum << 1) || (checksum >> 7); - checksum += data; - *(destination++) = data; - } - return (checksum == EEPROM.read(source)); -} -int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, unsigned int size) { - unsigned char data, checksum = 0; - for (; size > 0; size--) { - data = EEPROM.read(source++); - checksum = (checksum << 1) | (checksum >> 7); - checksum += data; - *(destination++) = data; - } - return (checksum == EEPROM.read(source)); -} - -// Read selected coordinate data from EEPROM. Updates pointed coord_data value. -// This is now a compatibility routine that is used to propagate coordinate data -// in the old EEPROM format to the new tagged NVS format. -bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data) { - uint32_t addr = coord_select * (sizeof(float) * N_AXIS + 1) + EEPROM_ADDR_PARAMETERS; - if (!(memcpy_from_eeprom_with_old_checksum((char*)coord_data, addr, sizeof(float) * N_AXIS)) && - !(memcpy_from_eeprom_with_checksum((char*)coord_data, addr, sizeof(float) * MAX_N_AXIS))) { - // Reset with default zero vector - clear_vector_float(coord_data); - // The old code used to rewrite the zeroed data but now that is done - // elsewhere, in a different format. - return false; - } - return true; -} - -// Allow iteration over CoordIndex values -CoordIndex& operator ++ (CoordIndex& i) { - i = static_cast(static_cast(i) + 1); - return i; -} diff --git a/Grbl_Esp32/src/Eeprom.h b/Grbl_Esp32/src/Eeprom.h deleted file mode 100644 index b3ed9998..00000000 --- a/Grbl_Esp32/src/Eeprom.h +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once - -/* - Eeprom.h - Header for system level commands and real-time processes - Part of Grbl - Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC - - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P - - Grbl is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -// #include "Grbl.h" - -// Define EEPROM memory address location values for saved coordinate data. -const int EEPROM_SIZE = 1024U; -const int EEPROM_ADDR_PARAMETERS = 512U; - -//unsigned char eeprom_get_char(unsigned int addr); -//void eeprom_put_char(unsigned int addr, unsigned char new_value); -void memcpy_to_eeprom_with_checksum(unsigned int destination, const char* source, unsigned int size); -int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, unsigned int size); -int memcpy_from_eeprom_with_old_checksum(char* destination, unsigned int source, unsigned int size); - -// Reads selected coordinate data from EEPROM -bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data); - -// Various places in the code access saved coordinate system data -// by a small integer index according to the values below. -enum CoordIndex : uint8_t{ - Begin = 0, - G54 = Begin, - G55, - G56, - G57, - G58, - G59, - // To support 9 work coordinate systems it would be necessary to define - // the following 3 and modify GCode.cpp to support G59.1, G59.2, G59.3 - // G59_1, - // G59_2, - // G59_3, - NWCSystems, - G28 = NWCSystems, - G30, - // G92_2, - // G92_3, - End, -}; -// Allow iteration over CoordIndex values -CoordIndex& operator ++ (CoordIndex& i); diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp index 970124cc..b0401848 100644 --- a/Grbl_Esp32/src/Error.cpp +++ b/Grbl_Esp32/src/Error.cpp @@ -61,16 +61,16 @@ std::map ErrorNames = { { Error::GcodeG43DynamicAxisError, "Gcode G43 dynamic axis error" }, { Error::GcodeMaxValueExceeded, "Gcode max value exceeded" }, { Error::PParamMaxExceeded, "P param max exceeded" }, - { Error::SdFailedMount, "SD failed mount" }, - { Error::SdFailedRead, "SD failed read" }, - { Error::SdFailedOpenDir, "SD failed to open directory" }, - { Error::SdDirNotFound, "SD directory not found" }, - { Error::SdFileEmpty, "SD file empty" }, - { Error::SdFileNotFound, "SD file not found" }, - { Error::SdFailedOpenFile, "SD failed to open file" }, - { Error::SdFailedBusy, "SD is busy" }, - { Error::SdFailedDelDir, "SD failed to delete directory" }, - { Error::SdFailedDelFile, "SD failed to delete file" }, + { Error::FsFailedMount, "Failed to mount device" }, + { Error::FsFailedRead, "Failed to read" }, + { Error::FsFailedOpenDir, "Failed to open directory" }, + { Error::FsDirNotFound, "Directory not found" }, + { Error::FsFileEmpty, "File empty" }, + { Error::FsFileNotFound, "File not found" }, + { Error::FsFailedOpenFile, "Failed to open file" }, + { Error::FsFailedBusy, "Device is busy" }, + { Error::FsFailedDelDir, "Failed to delete directory" }, + { Error::FsFailedDelFile, "Failed to delete file" }, { Error::BtFailBegin, "Bluetooth failed to start" }, { Error::WifiFailBegin, "WiFi failed to start" }, { Error::NumberRange, "Number out of range for setting" }, @@ -79,5 +79,7 @@ std::map ErrorNames = { { Error::NvsSetFailed, "Failed to store setting" }, { Error::NvsGetStatsFailed, "Failed to get setting status" }, { Error::AuthenticationFailed, "Authentication failed!" }, + { Error::AnotherInterfaceBusy, "Another interface is busy" }, { Error::BadPinSpecification, "Bad Pin Specification" }, + { Error::JogCancelled, "Jog Cancelled" }, }; diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h index 269c064b..733fd973 100644 --- a/Grbl_Esp32/src/Error.h +++ b/Grbl_Esp32/src/Error.h @@ -64,16 +64,16 @@ enum class Error : uint8_t { GcodeG43DynamicAxisError = 37, GcodeMaxValueExceeded = 38, PParamMaxExceeded = 39, - SdFailedMount = 60, // SD Failed to mount - SdFailedRead = 61, // SD Failed to read file - SdFailedOpenDir = 62, // SD card failed to open directory - SdDirNotFound = 63, // SD Card directory not found - SdFileEmpty = 64, // SD Card directory not found - SdFileNotFound = 65, // SD Card file not found - SdFailedOpenFile = 66, // SD card failed to open file - SdFailedBusy = 67, // SD card is busy - SdFailedDelDir = 68, - SdFailedDelFile = 69, + FsFailedMount = 60, // SD Failed to mount + FsFailedRead = 61, // SD Failed to read file + FsFailedOpenDir = 62, // SD card failed to open directory + FsDirNotFound = 63, // SD Card directory not found + FsFileEmpty = 64, // SD Card directory not found + FsFileNotFound = 65, // SD Card file not found + FsFailedOpenFile = 66, // SD card failed to open file + FsFailedBusy = 67, // SD card is busy + FsFailedDelDir = 68, + FsFailedDelFile = 69, BtFailBegin = 70, // Bluetooth failed to start WifiFailBegin = 71, // WiFi failed to start NumberRange = 80, // Setting number range problem @@ -83,6 +83,8 @@ enum class Error : uint8_t { NvsGetStatsFailed = 101, AuthenticationFailed = 110, Eol = 111, + AnotherInterfaceBusy = 120, + JogCancelled = 130, BadPinSpecification = 150, }; diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index bc2950b6..19a48f23 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -26,6 +26,12 @@ #include "MachineConfig.h" +// Allow iteration over CoordIndex values +CoordIndex& operator++(CoordIndex& i) { + i = static_cast(static_cast(i) + 1); + return i; +} + // NOTE: Max line number is defined by the g-code standard to be 99999. It seems to be an // arbitrary value, and some GUIs may require more. So we increased it based on a max safe // value when converting a float (7.2 digit precision)s to an integer. @@ -236,6 +242,7 @@ Error gc_execute_line(char* line, uint8_t client) { mantissa = 0; // Set to zero to indicate valid non-integer G command. break; default: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "M4 requires laser mode or a reversable spindle"); FAIL(Error::GcodeUnsupportedCommand); // not reached break; @@ -489,9 +496,7 @@ Error gc_execute_line(char* line, uint8_t client) { break; case 6: // tool change gc_block.modal.tool_change = ToolChange::Enable; -#ifdef USE_TOOL_CHANGE //user_tool_change(gc_state.tool); -#endif mg_word_bit = ModalGroup::MM6; break; case 7: @@ -1282,14 +1287,16 @@ Error gc_execute_line(char* line, uint8_t client) { FAIL(Error::InvalidJogCommand); } // Initialize planner data to current spindle and coolant modal state. - pl_data->spindle_speed = gc_state.spindle_speed; - pl_data->spindle = gc_state.modal.spindle; - pl_data->coolant = gc_state.modal.coolant; - Error status = jog_execute(pl_data, &gc_block); - if (status == Error::Ok) { + pl_data->spindle_speed = gc_state.spindle_speed; + pl_data->spindle = gc_state.modal.spindle; + pl_data->coolant = gc_state.modal.coolant; + bool cancelledInflight = false; + Error status = jog_execute(pl_data, &gc_block, &cancelledInflight); + if (status == Error::Ok && !cancelledInflight) { memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); } - return status; + // JogCancelled is not reported as a GCode error + return status == Error::JogCancelled ? Error::Ok : status; } // If in laser mode, setup laser power based on current and past parser conditions. if (MachineConfig::instance()->_laserMode) { @@ -1356,9 +1363,7 @@ Error gc_execute_line(char* line, uint8_t client) { // gc_state.tool = gc_block.values.t; // [6. Change tool ]: NOT SUPPORTED if (gc_block.modal.tool_change == ToolChange::Enable) { -#ifdef USE_TOOL_CHANGE user_tool_change(gc_state.tool); -#endif } // [7. Spindle control ]: if (gc_state.modal.spindle != gc_block.modal.spindle) { @@ -1396,10 +1401,11 @@ Error gc_execute_line(char* line, uint8_t client) { if ((gc_block.modal.io_control == IoControl::DigitalOnSync) || (gc_block.modal.io_control == IoControl::DigitalOffSync) || (gc_block.modal.io_control == IoControl::DigitalOnImmediate) || (gc_block.modal.io_control == IoControl::DigitalOffImmediate)) { if (gc_block.values.p < MaxUserDigitalPin) { - if (!sys_io_control( - bit((int)gc_block.values.p), - (gc_block.modal.io_control == IoControl::DigitalOnSync) || (gc_block.modal.io_control == IoControl::DigitalOnImmediate), - (gc_block.modal.io_control == IoControl::DigitalOnSync) || (gc_block.modal.io_control == IoControl::DigitalOffSync))) { + if ((gc_block.modal.io_control == IoControl::DigitalOnSync) || (gc_block.modal.io_control == IoControl::DigitalOffSync)) { + protocol_buffer_synchronize(); + } + bool turnOn = gc_block.modal.io_control == IoControl::DigitalOnSync || gc_block.modal.io_control == IoControl::DigitalOnImmediate; + if (!sys_set_digital((int)gc_block.values.p, turnOn)) { FAIL(Error::PParamMaxExceeded); } } else { @@ -1409,8 +1415,12 @@ Error gc_execute_line(char* line, uint8_t client) { if ((gc_block.modal.io_control == IoControl::SetAnalogSync) || (gc_block.modal.io_control == IoControl::SetAnalogImmediate)) { if (gc_block.values.e < MaxUserDigitalPin) { gc_block.values.q = constrain(gc_block.values.q, 0.0, 100.0); // force into valid range - if (!sys_pwm_control(bit((int)gc_block.values.e), gc_block.values.q, (gc_block.modal.io_control == IoControl::SetAnalogSync))) + if (gc_block.modal.io_control == IoControl::SetAnalogSync) { + protocol_buffer_synchronize(); + } + if (!sys_set_analog((int)gc_block.values.e, gc_block.values.q)) { FAIL(Error::PParamMaxExceeded); + } } else { FAIL(Error::PParamMaxExceeded); } @@ -1425,7 +1435,7 @@ Error gc_execute_line(char* line, uint8_t client) { #endif // [10. Dwell ]: if (gc_block.non_modal_command == NonModal::Dwell) { - mc_dwell(gc_block.values.p); + mc_dwell(int32_t(gc_block.values.p * 1000.0f)); } // [11. Set active plane ]: gc_state.modal.plane_select = gc_block.modal.plane_select; @@ -1475,9 +1485,9 @@ Error gc_execute_line(char* line, uint8_t client) { // and absolute and incremental modes. pl_data->motion.rapidMotion = 1; // Set rapid motion flag. if (axis_command != AxisCommand::None) { - mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); + cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } - mc_line_kins(coord_data, pl_data, gc_state.position); + cartesian_to_motors(coord_data, pl_data, gc_state.position); memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); break; case NonModal::SetHome0: @@ -1505,12 +1515,10 @@ Error gc_execute_line(char* line, uint8_t client) { if (axis_command == AxisCommand::MotionMode) { GCUpdatePos gc_update_pos = GCUpdatePos::Target; if (gc_state.modal.motion == Motion::Linear) { - //mc_line(gc_block.values.xyz, pl_data); - mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); + cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } else if (gc_state.modal.motion == Motion::Seek) { pl_data->motion.rapidMotion = 1; // Set rapid motion flag. - //mc_line(gc_block.values.xyz, pl_data); - mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); + cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } else if ((gc_state.modal.motion == Motion::CwArc) || (gc_state.modal.motion == Motion::CcwArc)) { mc_arc(gc_block.values.xyz, pl_data, @@ -1595,9 +1603,7 @@ Error gc_execute_line(char* line, uint8_t client) { MachineConfig::instance()->_coolant->off(); } report_feedback_message(Message::ProgramEnd); -#ifdef USE_M30 user_m30(); -#endif break; } gc_state.modal.program_flow = ProgramFlow::Running; // Reset program flow. diff --git a/Grbl_Esp32/src/GCode.h b/Grbl_Esp32/src/GCode.h index c637605d..64f09154 100644 --- a/Grbl_Esp32/src/GCode.h +++ b/Grbl_Esp32/src/GCode.h @@ -236,6 +236,32 @@ enum GCParserFlags { GCParserLaserIsMotion = bit(7), }; +// Various places in the code access saved coordinate system data +// by a small integer index according to the values below. +enum CoordIndex : uint8_t{ + Begin = 0, + G54 = Begin, + G55, + G56, + G57, + G58, + G59, + // To support 9 work coordinate systems it would be necessary to define + // the following 3 and modify GCode.cpp to support G59.1, G59.2, G59.3 + // G59_1, + // G59_2, + // G59_3, + NWCSystems, + G28 = NWCSystems, + G30, + // G92_2, + // G92_3, + End, +}; + +// Allow iteration over CoordIndex values +CoordIndex& operator ++ (CoordIndex& i); + // NOTE: When this struct is zeroed, the 0 values in the above types set the system defaults. typedef struct { Motion motion; // {G0,G1,G2,G3,G38.2,G80} diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index ca5d12c7..d6af9b3b 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -34,7 +34,8 @@ void grbl_init() { Serial.println("Initializing serial communications..."); // Setup serial baud rate and interrupts - serial_init(); + client_init(); + display_init(); grbl_msg_sendf( CLIENT_SERIAL, MsgLevel::Info, "Grbl_ESP32 Ver %s Date %s", GRBL_VERSION, GRBL_VERSION_BUILD); // print grbl_esp32 verion info grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Compiled with ESP32 SDK:%s", ESP.getSdkVersion()); // print the SDK version @@ -66,10 +67,8 @@ void grbl_init() { system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files) memset(sys_position, 0, sizeof(sys_position)); // Clear machine position. -#ifdef USE_MACHINE_INIT machine_init(); // user supplied function for special initialization -#endif - // Initialize system state. + // Initialize system state. #ifdef FORCE_INITIALIZATION_ALARM // Force Grbl into an ALARM state upon a power-cycle or hard reset. sys.state = State::Alarm; @@ -131,7 +130,7 @@ static void reset_variables() { sys_rt_s_override = SpindleSpeedOverride::Default; // Reset Grbl primary systems. - serial_reset_read_buffer(CLIENT_ALL); // Clear serial read buffer + client_reset_read_buffer(CLIENT_ALL); // Clear serial read buffer gc_init(); // Set g-code parser to default state spindle->stop(); @@ -144,6 +143,10 @@ static void reset_variables() { plan_sync_position(); gc_sync_position(); report_init_message(CLIENT_ALL); + + // used to keep track of a jog command sent to mc_line() so we can cancel it. + // this is needed if a jogCancel comes along after we have already parsed a jog and it is in-flight. + sys_pl_data_inflight = NULL; } void run_once() { @@ -161,6 +164,14 @@ void run_once() { } } +void __attribute__((weak)) machine_init() {} + +void __attribute__((weak)) display_init() {} + +void __attribute__((weak)) user_m30() {} + +void __attribute__((weak)) user_tool_change(uint8_t new_tool) {} + /* setup() and loop() in the Arduino .ino implements this control flow: diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 9ef5069b..9dfe14d7 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -41,7 +41,6 @@ const char* const GRBL_VERSION_BUILD = "20210326"; #include "Defaults.h" #include "Error.h" -#include "Eeprom.h" #include "WebUI/Authentication.h" #include "WebUI/Commands.h" #include "Probe.h" @@ -53,8 +52,9 @@ const char* const GRBL_VERSION_BUILD = "20210326"; #include "Limits.h" #include "MotionControl.h" #include "Protocol.h" -#include "Report.h" +#include "Uart.h" #include "Serial.h" +#include "Report.h" #include "Pin.h" #include "Spindles/Spindle.h" #include "Stepper.h" @@ -91,28 +91,22 @@ const char* const GRBL_VERSION_BUILD = "20210326"; void grbl_init(); void run_once(); -// Called if USE_MACHINE_INIT is defined +// Weak definitions that can be overridden void machine_init(); - -// Called if USE_CUSTOM_HOMING is defined bool user_defined_homing(uint8_t cycle_mask); -// Called if USE_KINEMATICS is defined +// weak definitions in MotionControl.cpp +bool inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position); +bool kinematics_pre_homing(uint8_t cycle_mask); +void kinematics_post_homing(); -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position); -bool kinematics_pre_homing(uint8_t cycle_mask); -void kinematics_post_homing(); -uint8_t kinematic_limits_check(float* target); +bool limitsCheckTravel(float* target); // weak in Limits.cpp; true if out of range -// Called if USE_FWD_KINEMATICS is defined void inverse_kinematics(float* position); // used to return a converted value void forward_kinematics(float* position); -// Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined void user_defined_macro(uint8_t index); -// Called if USE_M30 is defined void user_m30(); -// Called if USE_TOOL_CHANGE is defined void user_tool_change(uint8_t new_tool); diff --git a/Grbl_Esp32/src/I2SOut.cpp b/Grbl_Esp32/src/I2SOut.cpp index a81932f7..fb2ae70c 100644 --- a/Grbl_Esp32/src/I2SOut.cpp +++ b/Grbl_Esp32/src/I2SOut.cpp @@ -48,6 +48,7 @@ #include "WebUI/ESPResponse.h" #include "Probe.h" #include "System.h" +#include "Serial.h" #include "Report.h" #include @@ -534,7 +535,9 @@ static void IRAM_ATTR i2sOutTask(void* parameter) { I2S_OUT_PULSER_EXIT_CRITICAL(); // Unlock pulser status static UBaseType_t uxHighWaterMark = 0; +# ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +# endif } } #endif diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp index 1c0c3d00..03474c7a 100644 --- a/Grbl_Esp32/src/Jog.cpp +++ b/Grbl_Esp32/src/Jog.cpp @@ -24,11 +24,13 @@ #include "Grbl.h" // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. -Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { +// cancelledInflight will be set to true if was not added to parser due to a cancelJog. +Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* cancelledInflight) { // Initialize planner data struct for jogging motions. // NOTE: Spindle and coolant are allowed to fully function with overrides during a jog. pl_data->feed_rate = gc_block->values.f; pl_data->motion.noFeedOverride = 1; + pl_data->is_jog = true; #ifdef USE_LINE_NUMBERS pl_data->line_number = gc_block->values.n; #endif @@ -37,12 +39,10 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { return Error::TravelExceeded; } } -// Valid jog command. Plan, set state, and execute. -#ifndef USE_KINEMATICS - mc_line(gc_block->values.xyz, pl_data); -#else // else use kinematics - inverse_kinematics(gc_block->values.xyz, pl_data, gc_state.position); -#endif + // Valid jog command. Plan, set state, and execute. + if (!cartesian_to_motors(gc_block->values.xyz, pl_data, gc_state.position)) { + return Error::JogCancelled; + } if (sys.state == State::Idle) { if (plan_get_current_block() != NULL) { // Check if there is a block to execute. diff --git a/Grbl_Esp32/src/Jog.h b/Grbl_Esp32/src/Jog.h index 59307834..1867871e 100644 --- a/Grbl_Esp32/src/Jog.h +++ b/Grbl_Esp32/src/Jog.h @@ -28,4 +28,5 @@ const int JOG_LINE_NUMBER = 0; // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. -Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block); +// cancelledInflight will be set to true if was not added to parser due to a cancelJog. +Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* cancelledInflight); diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 2b01a681..ea541ad0 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -56,10 +56,12 @@ void IRAM_ATTR isr_limit_switches(void* /*unused */) { # ifdef HARD_LIMIT_FORCE_STATE_CHECK // Check limit pin state. if (limits_get_state()) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Debug, "Hard limits"); mc_reset(); // Initiate system kill. sys_rt_exec_alarm = ExecAlarm::HardLimit; // Indicate hard limit critical event } # else + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Debug, "Hard limits"); mc_reset(); // Initiate system kill. sys_rt_exec_alarm = ExecAlarm::HardLimit; // Indicate hard limit critical event # endif @@ -103,7 +105,6 @@ void limits_go_home(uint8_t cycle_mask) { // Initialize variables used for homing computations. uint8_t n_cycle = (2 * n_homing_locate_cycle + 1); uint8_t step_pin[MAX_N_AXIS]; - float target[MAX_N_AXIS]; float max_travel = 0.0; auto n_axis = MachineConfig::instance()->_axes->_numberAxis; @@ -111,8 +112,8 @@ void limits_go_home(uint8_t cycle_mask) { // Initialize step pin masks step_pin[idx] = bit(idx); if (bit_istrue(cycle_mask, bit(idx))) { - // Set target based on max_travel setting. Ensure homing switches engaged with search scalar. - max_travel = MAX(max_travel, (HOMING_AXIS_SEARCH_SCALAR)* MachineConfig::instance()->_axes->_axis[idx]->_maxTravel); + // Set target based on max_travel setting. Ensure homing switches engaged with search scalar. + max_travel = MAX(max_travel, (HOMING_AXIS_SEARCH_SCALAR)*MachineConfig::instance()->_axes->_axis[idx]->_maxTravel); } } // Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches. @@ -121,7 +122,7 @@ void limits_go_home(uint8_t cycle_mask) { uint8_t n_active_axis; AxisMask limit_state, axislock; do { - system_convert_array_steps_to_mpos(target, sys_position); + float* target = system_get_mpos(); // Initialize and declare variables needed for homing routine. axislock = 0; n_active_axis = 0; @@ -196,7 +197,8 @@ void limits_go_home(uint8_t cycle_mask) { if (sys_rt_exec_alarm != ExecAlarm::None) { MachineConfig::instance()->_axes->set_homing_mode(cycle_mask, false); // tell motors homing is done...failed - mc_reset(); // Stop motors, if they are running. + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Debug, "Homing fail"); + mc_reset(); // Stop motors, if they are running. protocol_execute_realtime(); return; } else { @@ -249,7 +251,7 @@ void limits_go_home(uint8_t cycle_mask) { } } } - sys.step_control = {}; // Return step control to normal operation. + sys.step_control = {}; // Return step control to normal operation. MachineConfig::instance()->_axes->set_homing_mode(cycle_mask, false); // tell motors homing is done } @@ -264,10 +266,8 @@ void limits_init() { for (int gang_index = 0; gang_index < 2; gang_index++) { Pin pin; if ((pin = LimitPins[axis][gang_index]->get()) != Pin::UNDEFINED) { - #ifndef DISABLE_LIMIT_PIN_PULL_UP - if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) - { + if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) { mode = mode | Pin::Attr::PullUp; } #endif @@ -356,6 +356,7 @@ void limits_soft_check(float* target) { } } while (sys.state != State::Idle); } + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Debug, "Soft limits"); mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. sys_rt_exec_alarm = ExecAlarm::SoftLimit; // Indicate soft limit critical event protocol_execute_realtime(); // Execute to enter critical event loop and system abort @@ -372,12 +373,14 @@ void limitCheckTask(void* pvParameters) { AxisMask switch_state; switch_state = limits_get_state(); if (switch_state) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Limit Switch State %08d", switch_state); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Debug, "Limit Switch State %08d", switch_state); mc_reset(); // Initiate system kill. sys_rt_exec_alarm = ExecAlarm::HardLimit; // Indicate hard limit critical event } static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +#endif } } @@ -395,15 +398,24 @@ float limitsMinPosition(uint8_t axis) { // Checks and reports if target array exceeds machine travel limits. // Return true if exceeding limits -bool limitsCheckTravel(float* target) { +// Set $/MaxTravel=0 to selectively remove an axis from soft limit checks +bool __attribute__((weak)) limitsCheckTravel(float* target) { uint8_t idx; auto n_axis = MachineConfig::instance()->_axes->_numberAxis; for (idx = 0; idx < n_axis; idx++) { float max_mpos, min_mpos; - if (target[idx] < limitsMinPosition(idx) || target[idx] > limitsMaxPosition(idx)) { + if ((target[idx] < limitsMinPosition(idx) || target[idx] > limitsMaxPosition(idx)) && axis_settings[idx]->max_travel->get() > 0) { return true; } } return false; } + +bool limitsSwitchDefined(uint8_t axis, uint8_t gang_index) { + return (limit_pins[axis][gang_index] != UNDEFINED_PIN); +} + +bool __attribute__((weak)) user_defined_homing(uint8_t cycle_mask) { + return false; +} diff --git a/Grbl_Esp32/src/Limits.h b/Grbl_Esp32/src/Limits.h index 29ec710f..db22dde2 100644 --- a/Grbl_Esp32/src/Limits.h +++ b/Grbl_Esp32/src/Limits.h @@ -54,3 +54,6 @@ float limitsMinPosition(uint8_t axis); // Internal factor used by limits_soft_check bool limitsCheckTravel(float* target); + +// check if a switch has been defined +bool limitsSwitchDefined(uint8_t axis, uint8_t gang_index); diff --git a/Grbl_Esp32/src/Machines/3axis_rs485.h b/Grbl_Esp32/src/Machines/3axis_rs485.h deleted file mode 100644 index 48581b93..00000000 --- a/Grbl_Esp32/src/Machines/3axis_rs485.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once -// clang-format off - -/* - 3axis_xyx.h - Part of Grbl_ESP32 - - This is a general XYZ-axis RS-485 CNC machine. The schematic is quite - easy, you basically need a MAX485 wired through a logic level converter - for the VFD, and a few pins wired through an ULN2803A to the external - stepper drivers. It's common to have a dual gantry for the Y axis. - - Optional limit pins are slightly more difficult, as these require a - Schmitt trigger and optocouplers. - - 2020 - Stefan de Bruijn - - Grbl_ESP32 is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "ESP32_XYZ_RS485" -#define X_STEP_PIN "gpio.4" // labeled X -#define X_DIRECTION_PIN "gpio.16" // labeled X -#define Y_STEP_PIN "gpio.17" // labeled Y -#define Y_DIRECTION_PIN "gpio.18" // labeled Y -#define Y2_STEP_PIN "gpio.19" // labeled Y2 -#define Y2_DIRECTION_PIN "gpio.21" // labeled Y2 -#define Z_STEP_PIN "gpio.22" // labeled Z -#define Z_DIRECTION_PIN "gpio.23" // labeled Z - -#define SPINDLE_TYPE SpindleType::H2A -#define VFD_RS485_TXD_PIN "gpio.13" // RS485 TX -#define VFD_RS485_RTS_PIN "gpio.15" // RS485 RTS -#define VFD_RS485_RXD_PIN "gpio.2" // RS485 RX - -#define X_LIMIT_PIN "gpio.33" -#define Y_LIMIT_PIN "gpio.32" -#define Y2_LIMIT_PIN "gpio.35" -#define Z_LIMIT_PIN "gpio.34" - -// Set $Homing/Cycle0=X and $Homing/Cycle=XY - -#define PROBE_PIN "gpio.14" // labeled Probe -#define CONTROL_RESET_PIN "gpio.27" // labeled Reset -#define CONTROL_FEED_HOLD_PIN "gpio.26" // labeled Hold -#define CONTROL_CYCLE_START_PIN "gpio.25" // labeled Start - -// #define VFD_DEBUG_MODE diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h deleted file mode 100644 index 2ad008eb..00000000 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ /dev/null @@ -1,123 +0,0 @@ -#pragma once -// clang-format off - -/* - 6_pack_stepstick_XYZ_v1.h - - Covers all V1 versions V1p0, V1p1, etc - - Part of Grbl_ESP32 - Pin assignments for the ESP32 I2S 6-axis board - 2018 - Bart Dring - 2020 - Mitch Bradley - 2020 - Michiyasu Odaki - 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 StepStick XYZ" - -#define N_AXIS 3 - -// === Special Features - -// I2S (steppers & other output-only pins) -#define USE_I2S_OUT -#define USE_I2S_STEPS -//#define DEFAULT_STEPPER ST_I2S_STATIC -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE - -#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set - -#define I2S_OUT_BCK "gpio.22" -#define I2S_OUT_WS "gpio.17" -#define I2S_OUT_DATA "gpio.21" - - -// Motor Socket #1 -#define X_DISABLE_PIN "i2so.0" -#define X_DIRECTION_PIN "i2so.1" -#define X_STEP_PIN "i2so.2" - -// Motor Socket #2 -#define Y_DIRECTION_PIN "i2so.4" -#define Y_STEP_PIN "i2so.5" -#define Y_DISABLE_PIN "i2so.7" - -// Motor Socket #3 -#define Z_DISABLE_PIN "i2so.8" -#define Z_DIRECTION_PIN "i2so.9" -#define Z_STEP_PIN "i2so.10" - -/* - Socket I/O reference - The list of modules is here... - https://github.com/bdring/6-Pack_CNC_Controller/wiki/CNC-I-O-Module-List - Click on each module to get example for using the modules in the sockets - - -Socket #1 -#1 "gpio.33" -#2 "gpio.32" -#3 "gpio.35" (input only) -#4 "gpio.34" (input only) - -Socket #2 -#1 "gpio.2" -#2 "gpio.25" -#3 "gpio.39" (input only) -#4 "gpio.36" (input only) - -Socket #3 -#1 "gpio.26" -#2 "gpio.4" -#3 "gpio.16" -#4 "gpio.27" - -Socket #4 -#1 "gpio.14" -#2 "gpio.13" -#3 "gpio.15" -#4 "gpio.12" - -Socket #5 -#1 "i2so.24" (output only) -#2 "i2so.25" (output only) -#3 "i2so.26" (output only) - -*/ - - -// 4x Input Module in Socket #1 -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -#define X_LIMIT_PIN "gpio.33" -#define Y_LIMIT_PIN "gpio.32" -#define Z_LIMIT_PIN "gpio.35" - - -// // 4x Input Module in Socket #2 -// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -// #define X_LIMIT_PIN "gpio.2" -// #define Y_LIMIT_PIN "gpio.25" -// #define Z_LIMIT_PIN "gpio.39" - -// // 4x Input Module in Socket #3 -// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -// #define CONTROL_CYCLE_START_PIN "gpio.26" -// #define CONTROL_FEED_HOLD_PIN "gpio.4" -// #define CONTROL_RESET_PIN "gpio.16" -// #define CONTROL_SAFETY_DOOR_PIN "gpio.27" -// //#define INVERT_CONTROL_PIN_MASK B0000 - -// ================= Setting Defaults ========================== -#define DEFAULT_X_STEPS_PER_MM 800 -#define DEFAULT_Y_STEPS_PER_MM 800 -#define DEFAULT_Z_STEPS_PER_MM 800 diff --git a/Grbl_Esp32/src/Machines/Root_Controller_Root_4_Lite_RS485.h b/Grbl_Esp32/src/Machines/Root_Controller_Root_4_Lite_RS485.h new file mode 100644 index 00000000..f9bb81ca --- /dev/null +++ b/Grbl_Esp32/src/Machines/Root_Controller_Root_4_Lite_RS485.h @@ -0,0 +1,136 @@ +#pragma once +// clang-format off + +/* + Root_Controller_Root_4_Lite_RS485.h + + Covers initial release version + + Part of Grbl_ESP32 + Pin assignments for the ESP32 I2S Root Controller 6-axis board + 2018 - Bart Dring + 2020 - Mitch Bradley + 2020 - Michiyasu Odaki + 2020 - Pete Newbery + 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 "Root Controller 3 Axis XYYZ" + +#define N_AXIS 3 + +// === Special Features + +//**I2S (steppers & other output-only pins) +#define USE_I2S_OUT +#define USE_I2S_STEPS +//#define DEFAULT_STEPPER ST_I2S_STATIC +#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set + +//Setup I2S Clocking +#define I2S_OUT_BCK "gpio.22" +#define I2S_OUT_WS "gpio.21" +#define I2S_OUT_DATA "gpio.12" + + +//**Motor Socket #1 +#define X_DISABLE_PIN "i2so.7" +#define X_DIRECTION_PIN "i2so.6" +#define X_STEP_PIN "i2so.5" +//**Motor Socket #2 +#define Y_DISABLE_PIN "i2so.4" +#define Y_DIRECTION_PIN "i2so.3" +#define Y_STEP_PIN "i2so.2" +//**Motor Socket #3 +#define Y2_DISABLE_PIN "i2so.1" +#define Y2_DIRECTION_PIN "i2so.0" +#define Y2_STEP_PIN "i2so.15" +//**Motor Socket #4 +#define Z_DISABLE_PIN "i2so.14" +#define Z_DIRECTION_PIN "i2so.13" +#define Z_STEP_PIN "i2so.12" + +//**Motion Control +//200pulses/rev stepper motor with SFU1204 ballscrew with a pitch of 4mm +//equates to 50 Steps/mm * micro stepping +//Steps per MM +#define DEFAULT_X_STEPS_PER_MM 800 +#define DEFAULT_Y_STEPS_PER_MM 800 +#define DEFAULT_Z_STEPS_PER_MM 1000 // 50 Steps/mm * micro stepping * belt ratio +//**Max Feedrate +#define DEFAULT_X_MAX_RATE 1000 +#define DEFAULT_Y_MAX_RATE 1000 +#define DEFAULT_Z_MAX_RATE 1000 +//**Acceleration +#define DEFAULT_X_ACCELERATION 50 +#define DEFAULT_Y_ACCELERATION 50 +#define DEFAULT_Z_ACCELERATION 50 +//**Max travel +#define DEFAULT_X_MAX_TRAVEL 220 +#define DEFAULT_Y_MAX_TRAVEL 278 +#define DEFAULT_Z_MAX_TRAVEL 60 + + +//**Storage +#define ENABLE_SD_CARD + +//**Endstop pins +#define X_LIMIT_PIN "gpio.2" +#define Y_LIMIT_PIN "gpio.26" +#define Y2_LIMIT_PIN "gpio.27" +#define Z_LIMIT_PIN "gpio.14" +#define PROBE_PIN "gpio.33" +#define INVERT_CONTROL_PIN_MASK 1 +#define DEFAULT_INVERT_LIMIT_PINS 7 // Enable for NC switch types +//**Homing Routine +#define DEFAULT_HOMING_ENABLE 1 +#define DEFAULT_HOMING_DIR_MASK 0 +#define DEFAULT_SOFT_LIMIT_ENABLE 1 +#define DEFAULT_HARD_LIMIT_ENABLE 1 +#define DEFAULT_HOMING_SQUARED_AXES (bit(Y_AXIS)) +#define DEFAULT_HOMING_FEED_RATE 100 +#define DEFAULT_HOMING_SEEK_RATE 800 +#define DEFAULT_HOMING_PULLOFF 2 +#define HOMING_INIT_LOCK +#define LIMITS_TWO_SWITCHES_ON_AXES 1 +#define DEFAULT_HOMING_CYCLE_0 (1<. +*/ + +#define MACHINE_NAME "TMC2209 4x Controller" + +#define N_AXIS 4 + +#define TRINAMIC_UART_RUN_MODE TrinamicUartMode :: StealthChop +#define TRINAMIC_UART_HOMING_MODE TrinamicUartMode :: StallGuard + +#define TMC_UART UART_NUM_1 +#define TMC_UART_RX "gpio.21" +#define TMC_UART_TX "gpio.22" + +#define X_TRINAMIC_DRIVER 2209 +#define X_STEP_PIN "gpio.26" +#define X_DIRECTION_PIN "gpio.27" +#define X_RSENSE TMC2209_RSENSE_DEFAULT +#define X_DRIVER_ADDRESS 0 +#define DEFAULT_X_MICROSTEPS 16 + +#define Y_TRINAMIC_DRIVER 2209 +#define Y_STEP_PIN "gpio.33" +#define Y_DIRECTION_PIN "gpio.32" +#define Y_RSENSE TMC2209_RSENSE_DEFAULT +#define Y_DRIVER_ADDRESS 1 +#define DEFAULT_Y_MICROSTEPS 16 + +#define Z_TRINAMIC_DRIVER 2209 +#define Z_STEP_PIN "gpio.2" +#define Z_DIRECTION_PIN "gpio.14" +#define Z_RSENSE TMC2209_RSENSE_DEFAULT +#define Z_DRIVER_ADDRESS 2 +#define DEFAULT_Z_MICROSTEPS 16 + +#define A_TRINAMIC_DRIVER 2209 +#define A_STEP_PIN "gpio.16" +#define A_DIRECTION_PIN "gpio.15" +#define A_RSENSE TMC2209_RSENSE_DEFAULT +#define A_DRIVER_ADDRESS 3 +#define DEFAULT_A_MICROSTEPS 16 + +#define X_LIMIT_PIN "gpio.35" +#define Y_LIMIT_PIN "gpio.34" +#define Z_LIMIT_PIN "gpio.39" +#define PROBE_PIN "gpio.36" + +// OK to comment out to use pin for other features +#define STEPPERS_DISABLE_PIN "gpio.25" + + +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/Quad-MOSFET-Module +#define USER_DIGITAL_PIN_0 "gpio.4" // M62 M63 +#define USER_DIGITAL_PIN_1 "gpio.13" // M62 M63 +#define USER_DIGITAL_PIN_2 "gpio.17" // M62 M63 +#define USER_DIGITAL_PIN_3 "gpio.12" // M62 M63 + + +// ===================== defaults ====================== +// https://github.com/bdring/Grbl_Esp32/wiki/Setting-Defaults + +#define DEFAULT_INVERT_PROBE_PIN 1 diff --git a/Grbl_Esp32/src/Machines/atari_1020.h b/Grbl_Esp32/src/Machines/atari_1020.h index b82b1770..5978e7c5 100644 --- a/Grbl_Esp32/src/Machines/atari_1020.h +++ b/Grbl_Esp32/src/Machines/atari_1020.h @@ -140,11 +140,7 @@ #define ATARI_HOMING_ATTEMPTS 13 // tells grbl we have some special functions to call -#define USE_MACHINE_INIT -#define USE_CUSTOM_HOMING -#define USE_TOOL_CHANGE #define ATARI_TOOL_CHANGE_Z 5.0 -#define USE_M30 // use the user defined end of program #ifndef atari_h #define atari_h diff --git a/Grbl_Esp32/src/Machines/fysetc_ant.h b/Grbl_Esp32/src/Machines/fysetc_ant.h new file mode 100644 index 00000000..79607088 --- /dev/null +++ b/Grbl_Esp32/src/Machines/fysetc_ant.h @@ -0,0 +1,85 @@ +#pragma once +// clang-format off + +/* + fysetc_ant.h + https://github.com/FYSETC/FYSETC-E4 + + 2020-12-03 B. Dring + + This is a machine definition file to use the FYSETC E4 3D Printer controller + This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used. + There are 5 inputs + The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to + spindle, mist and flood coolant to drive an external relay. + + Grbl_ESP32 is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl_ESP32. If not, see . +*/ + +#define MACHINE_NAME "FYSETC E4 3D Printer Controller" + +#define N_AXIS 4 + +#define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp" + +#define USE_KINEMATICS // there are kinematic equations for this machine + +#define TRINAMIC_RUN_MODE TrinamicMode :: StealthChop +#define TRINAMIC_HOMING_MODE TrinamicMode :: StealthChop + +#define TMC_UART UART_NUM_1 +#define TMC_UART_RX GPIO_NUM_21 +#define TMC_UART_TX GPIO_NUM_22 + +#define X_TRINAMIC_DRIVER 2209 +#define X_STEP_PIN GPIO_NUM_27 +#define X_DIRECTION_PIN GPIO_NUM_26 +#define X_RSENSE TMC2209_RSENSE_DEFAULT +#define X_DRIVER_ADDRESS 1 +#define DEFAULT_X_MICROSTEPS 16 + +#define Y_TRINAMIC_DRIVER 2209 +#define Y_STEP_PIN GPIO_NUM_33 +#define Y_DIRECTION_PIN GPIO_NUM_32 +#define Y_RSENSE TMC2209_RSENSE_DEFAULT +#define Y_DRIVER_ADDRESS 3 +#define DEFAULT_Y_MICROSTEPS 16 + +#define Z_TRINAMIC_DRIVER 2209 +#define Z_STEP_PIN GPIO_NUM_14 +#define Z_DIRECTION_PIN GPIO_NUM_12 +#define Z_RSENSE TMC2209_RSENSE_DEFAULT +#define Z_DRIVER_ADDRESS 0 +#define DEFAULT_Z_MICROSTEPS 16 + +#define A_TRINAMIC_DRIVER 2209 +#define A_STEP_PIN GPIO_NUM_16 +#define A_DIRECTION_PIN GPIO_NUM_17 +#define A_RSENSE TMC2209_RSENSE_DEFAULT +#define A_DRIVER_ADDRESS 2 +#define DEFAULT_A_MICROSTEPS 16 + +#define X_LIMIT_PIN GPIO_NUM_34 +#define Y_LIMIT_PIN GPIO_NUM_35 +#define Z_LIMIT_PIN GPIO_NUM_15 +#define A_LIMIT_PIN GPIO_NUM_36 // Labeled TB +#define PROBE_PIN GPIO_NUM_39 // Labeled TE + +// OK to comment out to use pin for other features +#define STEPPERS_DISABLE_PIN GPIO_NUM_25 + +#define SPINDLE_TYPE SpindleType::RELAY +#define SPINDLE_OUTPUT_PIN GPIO_NUM_13 // labeled Fan +#define COOLANT_MIST_PIN GPIO_NUM_2 // Labeled Hotbed +#define COOLANT_FLOOD_PIN GPIO_NUM_4 // Labeled Heater diff --git a/Grbl_Esp32/src/Machines/fysetc_e4.h b/Grbl_Esp32/src/Machines/fysetc_e4.h new file mode 100644 index 00000000..2ca9f9bb --- /dev/null +++ b/Grbl_Esp32/src/Machines/fysetc_e4.h @@ -0,0 +1,81 @@ +#pragma once +// clang-format off + +/* + fysetc_e4.h + https://github.com/FYSETC/FYSETC-E4 + + 2020-12-03 B. Dring + + This is a machine definition file to use the FYSETC E4 3D Printer controller + This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used. + There are 5 inputs + The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to + spindle, mist and flood coolant to drive an external relay. + + Grbl_ESP32 is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl_ESP32. If not, see . +*/ + +#define MACHINE_NAME "FYSETC E4 3D Printer Controller" + +#define N_AXIS 4 + +#define TRINAMIC_RUN_MODE TrinamicMode :: StealthChop +#define TRINAMIC_HOMING_MODE TrinamicMode :: StealthChop + +#define TMC_UART UART_NUM_1 +#define TMC_UART_RX "gpio.21" +#define TMC_UART_TX "gpio.22 " + +#define X_TRINAMIC_DRIVER 2209 +#define X_STEP_PIN "gpio.27" +#define X_DIRECTION_PIN "gpio.26" +#define X_RSENSE TMC2209_RSENSE_DEFAULT +#define X_DRIVER_ADDRESS 1 +#define DEFAULT_X_MICROSTEPS 16 + +#define Y_TRINAMIC_DRIVER 2209 +#define Y_STEP_PIN "gpio.33" +#define Y_DIRECTION_PIN "gpio.32" +#define Y_RSENSE TMC2209_RSENSE_DEFAULT +#define Y_DRIVER_ADDRESS 3 +#define DEFAULT_Y_MICROSTEPS 16 + +#define Z_TRINAMIC_DRIVER 2209 +#define Z_STEP_PIN "gpio.14" +#define Z_DIRECTION_PIN "gpio.12" +#define Z_RSENSE TMC2209_RSENSE_DEFAULT +#define Z_DRIVER_ADDRESS 0 +#define DEFAULT_Z_MICROSTEPS 16 + +#define A_TRINAMIC_DRIVER 2209 +#define A_STEP_PIN "gpio.16" +#define A_DIRECTION_PIN "gpio.17" +#define A_RSENSE TMC2209_RSENSE_DEFAULT +#define A_DRIVER_ADDRESS 2 +#define DEFAULT_A_MICROSTEPS 16 + +#define X_LIMIT_PIN "gpio.34" +#define Y_LIMIT_PIN "gpio.35" +#define Z_LIMIT_PIN "gpio.15" +#define A_LIMIT_PIN "gpio.36" // Labeled TB +#define PROBE_PIN "gpio.39" // Labeled TE + +// OK to comment out to use pin for other features +#define STEPPERS_DISABLE_PIN "gpio.25" + +#define SPINDLE_TYPE SpindleType::RELAY +#define SPINDLE_OUTPUT_PIN "gpio.13" // labeled Fan +#define COOLANT_MIST_PIN "gpio.2" // Labeled Hotbed +#define COOLANT_FLOOD_PIN "gpio.4" // Labeled Heater diff --git a/Grbl_Esp32/src/Machines/i2s_out_xxyyzz.h b/Grbl_Esp32/src/Machines/i2s_out_xxyyzz.h deleted file mode 100644 index 9cdb982d..00000000 --- a/Grbl_Esp32/src/Machines/i2s_out_xxyyzz.h +++ /dev/null @@ -1,102 +0,0 @@ -#pragma once -// clang-format off - -/* - i2s_out_xxyyzz.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 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_STEPS -//#define DEFAULT_STEPPER ST_I2S_STATIC - -#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set - -#define I2S_OUT_BCK "gpio.22" -#define I2S_OUT_WS "gpio.17" -#define I2S_OUT_DATA "gpio.21" - - -#define STEPPER_MS1 "gpio.23" // MOSI -#define STEPPER_MS2 "gpio.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.19" - -#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 SpindleType::PWM // only one spindle at a time -#define SPINDLE_OUTPUT_PIN "gpio.26" -#define SPINDLE_ENABLE_PIN "gpio.4" -#define SPINDLE_DIR_PIN "gpio.16" - -#define X_LIMIT_PIN "gpio.36" -#define Y_LIMIT_PIN "gpio.39" -#define Z_LIMIT_PIN "gpio.34" -//#define A_LIMIT_PIN "gpio.35" -//#define B_LIMIT_PIN "gpio.32" -//#define C_LIMIT_PIN "gpio.33" - -#define PROBE_PIN "gpio.25" - -#define COOLANT_MIST_PIN "gpio.2" - -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/src/Machines/i2s_out_xyzabc.h b/Grbl_Esp32/src/Machines/i2s_out_xyzabc.h deleted file mode 100644 index 867f5e1e..00000000 --- a/Grbl_Esp32/src/Machines/i2s_out_xyzabc.h +++ /dev/null @@ -1,105 +0,0 @@ -#pragma once -// clang-format off - -/* - 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_STEPS -//#define DEFAULT_STEPPER ST_I2S_STATIC - -#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set - -#define I2S_OUT_BCK "gpio.22" -#define I2S_OUT_WS "gpio.17" -#define I2S_OUT_DATA "gpio.21" - - -#define STEPPER_MS1 "gpio.23" // MOSI -#define STEPPER_MS2 "gpio.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.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 SpindleType::PWM // only one spindle at a time -#define SPINDLE_OUTPUT_PIN "gpio.26" -#define SPINDLE_ENABLE_PIN "gpio.4" -#define SPINDLE_DIR_PIN "gpio.16" - -#define X_LIMIT_PIN "gpio.36" -#define Y_LIMIT_PIN "gpio.39" -#define Z_LIMIT_PIN "gpio.34" -#define A_LIMIT_PIN "gpio.35" -#define B_LIMIT_PIN "gpio.32" -#define C_LIMIT_PIN "gpio.33" - -#define PROBE_PIN "gpio.25" - -#define COOLANT_MIST_PIN "gpio.2" - - - -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h b/Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h deleted file mode 100644 index 0da63d73..00000000 --- a/Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h +++ /dev/null @@ -1,106 +0,0 @@ -#pragma once -// clang-format off - -/* - 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_STEPS -//#define DEFAULT_STEPPER ST_I2S_STATIC - -#define I2S_OUT_BCK "gpio.22" -#define I2S_OUT_WS "gpio.17" -#define I2S_OUT_DATA "gpio.21" - -#define TRINAMIC_RUN_MODE TrinamicMode :: CoolStep -#define TRINAMIC_HOMING_MODE TrinamicMode :: 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 SpindleType::PWM // only one spindle at a time -#define SPINDLE_OUTPUT_PIN "gpio.26" -#define SPINDLE_ENABLE_PIN "gpio.4" -#define SPINDLE_DIR_PIN "gpio.16" -*/ -#define X_LIMIT_PIN "gpio.33" -#define Y_LIMIT_PIN "gpio.32" -#define Z_LIMIT_PIN "gpio.35" -#define A_LIMIT_PIN "gpio.34" -#define B_LIMIT_PIN "gpio.39" -#define C_LIMIT_PIN "gpio.36" - -#define SPINDLE_TYPE SpindleType::RELAY -#define SPINDLE_OUTPUT_PIN "gpio.26" - -#define PROBE_PIN "gpio.25" - -#define COOLANT_MIST_PIN "gpio.2" - -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h index ab584d4b..1f9bfc70 100644 --- a/Grbl_Esp32/src/Machines/midtbot.h +++ b/Grbl_Esp32/src/Machines/midtbot.h @@ -29,11 +29,7 @@ #define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp" -#define MIDTBOT // applies the geometry correction to the kinematics -#define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine -#define USE_CUSTOM_HOMING +#define MIDTBOT // applies the geometry correction to the kinematics #define SPINDLE_TYPE SpindleType::NONE @@ -50,8 +46,6 @@ #define Z_SERVO_PIN "gpio.27" -// Set $Homing/Cycle0=Y and $Homing/Cycle1=X - #define SPINDLE_TYPE SpindleType::NONE // defaults diff --git a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h index e45c658d..257db30e 100644 --- a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h +++ b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h @@ -30,7 +30,6 @@ #define MACHINE_NAME "MPCNC_V1P2 with Laser Module" // The laser module fires without a low signal. This keeps the enable on -#define USE_MACHINE_INIT #define LVL_SHIFT_ENABLE "gpio.32" #define CUSTOM_CODE_FILENAME "Custom/mpcnc_laser_module.cpp" @@ -53,8 +52,8 @@ // OK to comment out to use pin for other features #define STEPPERS_DISABLE_PIN "gpio.13" -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN "gpio.16" +#define SPINDLE_TYPE SpindleType::Laser +#define LASER_OUTPUT_PIN "gpio.16" #define COOLANT_MIST_PIN "gpio.2" diff --git a/Grbl_Esp32/src/Machines/polar_coaster.h b/Grbl_Esp32/src/Machines/polar_coaster.h index 0b6571ef..c367b36f 100644 --- a/Grbl_Esp32/src/Machines/polar_coaster.h +++ b/Grbl_Esp32/src/Machines/polar_coaster.h @@ -37,9 +37,6 @@ #define POLAR_AXIS 1 #define SEGMENT_LENGTH 0.5 // segment length in mm -#define USE_KINEMATICS -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_M30 #define X_STEP_PIN "gpio.15" #define Y_STEP_PIN "gpio.2" @@ -123,4 +120,6 @@ #define DEFAULT_X_MAX_TRAVEL 50.0 // mm NOTE: Must be a positive value. #define DEFAULT_Y_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value. -#define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode +#define DEFAULT_Z_MAX_TRAVEL 5.0 // This is percent in servo mode + +#define DEFAULT_Z_HOMING_MPOS DEFAULT_Z_MAX_TRAVEL // stays up after homing diff --git a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h deleted file mode 100644 index fcac8899..00000000 --- a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h +++ /dev/null @@ -1,84 +0,0 @@ -#pragma once -// clang-format off - -/* - spi_daisy_4axis_xyz.h - Part of Grbl_ESP32 - - Pin assignments for a 4-axis machine using Triaminic drivers - in daisy-chained SPI mode. - https://github.com/bdring/4_Axis_SPI_CNC - - 2019 - Bart Dring - 2020 - Mitch Bradley - - Grbl_ESP32 is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "SPI_DAISY_4X_XYZ" - -#ifdef N_AXIS - #undef N_AXIS -#endif -#define N_AXIS 3 - -#define TRINAMIC_DAISY_CHAIN - -#define TRINAMIC_RUN_MODE TrinamicMode :: CoolStep -#define TRINAMIC_HOMING_MODE TrinamicMode :: CoolStep - -// Use SPI enable instead of the enable pin -// The hardware enable pin is tied to ground -#define USE_TRINAMIC_ENABLE - -#define X_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define X_RSENSE TMC2130_RSENSE_DEFAULT -#define X_STEP_PIN "gpio.12" -#define X_DIRECTION_PIN "gpio.14" -#define X_CS_PIN "gpio.17" // Daisy Chain, all share same CS pin - -#define Y_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define Y_RSENSE TMC2130_RSENSE_DEFAULT -#define Y_STEP_PIN "gpio.27" -#define Y_DIRECTION_PIN "gpio.26" -#define Y_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin - -#define Z_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define Z_RSENSE TMC2130_RSENSE_DEFAULT -#define Z_STEP_PIN "gpio.15" -#define Z_DIRECTION_PIN "gpio.2" -#define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin - - -// Mist is a 3.3V output -// Turn on with M7 and off with M9 -#define COOLANT_MIST_PIN "gpio.21" - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN "gpio.25" -#define SPINDLE_ENABLE_PIN "gpio.4" - -// Relay operation -// Install Jumper near relay -// For spindle Use max RPM of 1 -// For PWM remove jumper and set MAX RPM to something higher ($30 setting) -// Interlock jumper along top edge needs to be installed for both versions -#define DEFAULT_SPINDLE_RPM_MAX 1 // Should be 1 for relay operation - -#define PROBE_PIN "gpio.22" - -#define X_LIMIT_PIN "gpio.36" -#define Y_LIMIT_PIN "gpio.39" -#define Z_LIMIT_PIN "gpio.34" - diff --git a/Grbl_Esp32/src/Machines/tapster_3.h b/Grbl_Esp32/src/Machines/tapster_3.h index 0eb185af..cbd23550 100644 --- a/Grbl_Esp32/src/Machines/tapster_3.h +++ b/Grbl_Esp32/src/Machines/tapster_3.h @@ -23,12 +23,6 @@ #define N_AXIS 3 -// ================= Firmware Customization =================== - -#define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine - // ================== Delta Geometry =========================== #define RADIUS_FIXED 70.0 // radius of the fixed side (length of motor cranks) diff --git a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h index 09597403..66d5092d 100644 --- a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h +++ b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h @@ -26,8 +26,6 @@ #define FWD_KINEMATICS_REPORTING // report in cartesian #define USE_RMT_STEPS // Use the RMT periferal to generate step pulses #define USE_TRINAMIC // some Trinamic motors are used on this machine -#define USE_MACHINE_TRINAMIC_INIT // there is a machine specific setup for the drivers -#define USE_MACHINE_INIT // There is some custom initialization for this machine #define SEGMENT_LENGTH 0.5 // segment length in mm #define KIN_ANGLE_CALC_OK 0 @@ -41,10 +39,6 @@ #define N_AXIS 3 -#define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine - // ================== Delta Geometry =========================== #define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks) diff --git a/Grbl_Esp32/src/Machines/template.h b/Grbl_Esp32/src/Machines/template.h deleted file mode 100644 index 60367c1d..00000000 --- a/Grbl_Esp32/src/Machines/template.h +++ /dev/null @@ -1,233 +0,0 @@ -#pragma once -// clang-format off - -/* - template.h - Part of Grbl_ESP32 - - Template for a machine configuration file. - - 2020 - Mitch Bradley - - Grbl_ESP32 is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl_ESP32. If not, see . -*/ - -// This contains a long list of things that might possibly be -// configured. Most machines - especially simple cartesian machines -// that use stepper motors - will only need to define a few of the -// options herein, often just the pin assignments. - -// Pin assignments depend on how the ESP32 is connected to -// the external machine. Typically the ESP32 module plugs into -// an adapter board that wires specific ESP32 GPIO pins to -// other connectors on the board, such as Pololu sockets for -// stepper drivers or connectors for external drivers, limit -// pins, spindle control, etc. This file describes how those -// GPIO pins are wired to those other connectors. - -// Some machines might choose to use an adapter board in a -// non-standard way, for example a 3-axis board might have axes -// labeled XYZ, but the machine might have only 2 axes one of which is -// driven by two ganged motors. In that case, you would need -// a custom version of this file that assigns the pins differently -// from the adapter board labels. - -// In addition to pin assignments, many other aspects of Grbl -// can be configured, such as spindle speeds, special motor -// types like servos and unipolars, homing order, default values -// for $$ settings, etc. A detailed list of such options is -// given below. - -// Furthermore, it is possible to implement special complex -// behavior in custom C++ code, for non-Cartesian machines, -// unusual homing cycles, etc. See the Special Features section -// below for additional instructions. - -// === Machine Name -// Change TEMPLATE to some name of your own choosing. That name -// will be shown in a Grbl startup message to identify your -// configuration. - -#define MACHINE_NAME "TEMPLATE" - -// If your machine requires custom code as described below in -// Special Features, you must copy Custom/custom_code_template.cpp -// to a new name like Custom/my_custom_code.cpp, implement the -// functions therein, and enable its use by defining: -// #define CUSTOM_CODE_FILENAME "Custom/my_custom_code.cpp" - -// === Number of axes - -// You can set the number of axes that the machine supports -// by defining N_AXIS. If you do not define it, 3 will be -// used. The value must be at least 3, even if your machine -// has fewer axes. -// #define N_AXIS 4 - - -// == Pin Assignments - -// Step and direction pins; these must be output-capable pins, -// specifically ESP32 GPIO numbers 0..31 -// #define X_STEP_PIN "gpio.12" -// #define X_DIRECTION_PIN "gpio.14" -// #define Y_STEP_PIN "gpio.26" -// #define Y_DIRECTION_PIN "gpio.15" -// #define Z_STEP_PIN "gpio.27" -// #define Z_DIRECTION_PIN "gpio.33" - -// #define X_LIMIT_PIN "gpio.17" -// #define Y_LIMIT_PIN "gpio.4" -// #define Z_LIMIT_PIN "gpio.16" - -// Common enable for all steppers. If it is okay to leave -// your drivers enabled at all times, you can leave -// STEPPERS_DISABLE_PIN undefined and use the pin for something else. -// #define STEPPERS_DISABLE_PIN "gpio.13" - -// Pins for controlling various aspects of the machine. If your -// machine does not support one of these features, you can leave -// the corresponding pin undefined. - -// #define SPINDLE_OUTPUT_PIN "gpio.2" // labeled SpinPWM -// #define SPINDLE_ENABLE_PIN "gpio.22" // labeled SpinEnbl -// #define COOLANT_MIST_PIN "gpio.21" // labeled Mist -// #define COOLANT_FLOOD_PIN "gpio.25" // labeled Flood -// #define PROBE_PIN "gpio.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.35" // labeled Door, needs external pullup - -// RESET, FEED_HOLD, and CYCLE_START can control GCode execution at -// the push of a button. - -// #define CONTROL_RESET_PIN "gpio.34" // labeled Reset, needs external pullup -// #define CONTROL_FEED_HOLD_PIN "gpio.36" // labeled Hold, needs external pullup -// #define CONTROL_CYCLE_START_PIN "gpio.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.27" /* labeled Z */ -// #define Y2_DIRECTION_PIN "gpio.33" /* labeled Z */ - -// === Servos -// To use a servo motor on an axis, do not define step and direction -// pins for that axis, but instead include a block like this: - -// #define SERVO_Z_PIN "gpio.22" - -// === Homing cycles -// Set them using $Homing/Cycle0= optionally up to $Homing/Cycle5= - -// === Default settings -// Grbl has many run-time settings that the user can changed by -// commands like $110=2000 . Their values are stored in non-volatile -// storage so they persist after the controller has been powered down. -// Those settings have default values that are used if the user -// has not altered them, or if the settings are explicitly reset -// to the default values wth $RST=$. -// -// The default values are established in defaults.h, but you -// can override any one of them by definining it here, for example: - -//#define DEFAULT_INVERT_LIMIT_PINS 1 -//#define DEFAULT_REPORT_INCHES 1 - -// === Control Pins - -// If some of the control pin switches are normally closed -// (the default is normally open), you can invert some of them -// with INVERT_CONTROL_PIN_MASK. The bits in the mask are -// Cycle Start, Feed Hold, Reset, Safety Door. To use a -// normally open switch on Reset, you would say -// #define INVERT_CONTROL_PIN_MASK B1101 - -// If your control pins do not have adequate hardware signal -// conditioning, you can define these to use software to -// reduce false triggering. -// #define ENABLE_CONTROL_SW_DEBOUNCE // Default disabled. Uncomment to enable. -// #define CONTROL_SW_DEBOUNCE_PERIOD 32 // in milliseconds default 32 microseconds - - -// Grbl_ESP32 use the ESP32's special RMT (IR remote control) hardware -// engine to achieve more precise high step rates than can be done -// in software. That feature is enabled by default, but there are -// some machines that might not want to use it, such as machines that -// do not use ordinary stepper motors. To turn it off, do this: -// #undef USE_RMT_STEPS - -// === Special Features -// Grbl_ESP32 can support non-Cartesian machines and some other -// scenarios that cannot be handled by choosing from a set of -// predefined selections. Instead they require machine-specific -// C++ code functions. There are callouts in the core code for -// such code, guarded by ifdefs that enable calling the individual -// functions. custom_code_template.cpp describes the functions -// that you can implement. The ifdef guards are described below: -// -// USE_CUSTOM_HOMING enables the user_defined_homing(uint8_t cycle_mask) function -// that can implement an arbitrary homing sequence. -// #define USE_CUSTOM_HOMING - -// USE_KINEMATICS enables the functions inverse_kinematics(), -// kinematics_pre_homing(), and kinematics_post_homing(), -// so non-Cartesian machines can be implemented. -// #define USE_KINEMATICS - -// USE_FWD_KINEMATICS enables the forward_kinematics() function -// that converts motor positions in non-Cartesian coordinate -// systems back to Cartesian form, for status reports. -//#define USE_FWD_KINEMATICS - -// USE_TOOL_CHANGE enables the user_tool_change() function -// that implements custom tool change procedures. -// #define USE_TOOL_CHANGE - -// Any one of MACRO_BUTTON_0_PIN, MACRO_BUTTON_1_PIN, and MACRO_BUTTON_2_PIN -// enables the user_defined_macro(number) function which -// implements custom behavior at the press of a button -// #define MACRO_BUTTON_0_PIN - -// USE_M30 enables the user_m30() function which implements -// custom behavior when a GCode programs stops at the end -// #define USE_M30 - -// USE_TRIAMINIC enables a suite of functions that are defined -// in grbl_triaminic.cpp, allowing the use of Triaminic stepper -// drivers that require software configuration at startup. -// There are several options that control the details of such -// drivers; inspect the code in grbl_triaminic.cpp to see them. -// #define USE_TRIAMINIC -// #define X_TRIAMINIC -// #define X_DRIVER_TMC2209 -// #define TRIAMINIC_DAISY_CHAIN - -// USE_MACHINE_TRINAMIC_INIT enables the machine_triaminic_setup() -// function that replaces the normal setup of Triaminic drivers. -// It could, for, example, setup StallGuard or other special modes. -// #define USE_MACHINE_TRINAMIC_INIT - - -// === Grbl behavior options -// There are quite a few options that control aspects of Grbl that -// are not specific to particular machines. They are listed and -// described in config.h after it includes the file machine.h. -// Normally you would not need to change them, but if you do, -// it will be necessary to make the change in config.h diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index c7ba7f52..c9d416c4 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -5,8 +5,8 @@ Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC Copyright (c) 2009-2011 Simen Svale Skogsrud - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P + 2018 - Bart Dring This file was modifed for use on the ESP32 + CPU. Do not use this with Grbl for atMega328P Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -34,15 +34,6 @@ SquaringMode ganged_mode = SquaringMode::Dual; -// this allows kinematics to be used. -void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position) { -#ifndef USE_KINEMATICS - mc_line(target, pl_data); -#else // else use kinematics - inverse_kinematics(target, pl_data, position); -#endif -} - // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. @@ -50,7 +41,12 @@ void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position) { // segments, must pass through this routine before being passed to the planner. The seperation of // mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being // in the planner and to let backlash compensation or canned cycle integration simple and direct. -void mc_line(float* target, plan_line_data_t* pl_data) { +// returns true if line was submitted to planner, or false if intentionally dropped. +bool mc_line(float* target, plan_line_data_t* pl_data) { + bool submitted_result = false; + // store the plan data so it can be cancelled by the protocol system if needed + sys_pl_data_inflight = pl_data; + // If enabled, check for soft limit violations. Placed here all line motions are picked up // from everywhere in Grbl. if (soft_limits->get()) { @@ -61,7 +57,8 @@ void mc_line(float* target, plan_line_data_t* pl_data) { } // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. if (sys.state == State::CheckMode) { - return; + sys_pl_data_inflight = NULL; + return submitted_result; // Bail, if system abort. } // NOTE: Backlash compensation may be installed here. It will need direction info to track when // to insert a backlash line motion(s) before the intended line motion and will require its own @@ -81,7 +78,8 @@ void mc_line(float* target, plan_line_data_t* pl_data) { do { protocol_execute_realtime(); // Check for any run-time commands if (sys.abort) { - return; // Bail, if system abort. + sys_pl_data_inflight = NULL; + return submitted_result; // Bail, if system abort. } if (plan_check_full_buffer()) { protocol_auto_cycle_start(); // Auto-cycle start when buffer is full. @@ -91,9 +89,29 @@ void mc_line(float* target, plan_line_data_t* pl_data) { } while (1); // Plan and queue motion into planner buffer // uint8_t plan_status; // Not used in normal operation. - plan_buffer_line(target, pl_data); + if (sys_pl_data_inflight == pl_data) { + plan_buffer_line(target, pl_data); + submitted_result = true; + } + sys_pl_data_inflight = NULL; + return submitted_result; } +bool __attribute__((weak)) cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { + return mc_line(target, pl_data); +} + +bool __attribute__((weak)) kinematics_pre_homing(uint8_t cycle_mask) { + return false; // finish normal homing cycle +} + +void __attribute__((weak)) kinematics_post_homing() {} + +void __attribute__((weak)) motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + memcpy(cartesian, motors, n_axis * sizeof(motors[0])); +} + +void __attribute__((weak)) forward_kinematics(float* position) {} // Execute an arc in offset mode format. position == current xyz, target == target xyz, // offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is // the direction of helical travel, radius == circle radius, isclockwise boolean. Used @@ -118,14 +136,13 @@ void mc_arc(float* target, float rt_axis1 = target[axis_1] - center_axis1; float previous_position[MAX_N_AXIS] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; -#ifdef USE_KINEMATICS uint16_t n; auto n_axis = MachineConfig::instance()->_axes->_numberAxis; for (n = 0; n < n_axis; n++) { previous_position[n] = position[n]; } -#endif + // CCW angle between position and target from circle center. Only one atan2() trig computation required. float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); if (is_clockwise_arc) { // Correct atan2 output per direction @@ -185,8 +202,9 @@ void mc_arc(float* target, float cos_Ti; float r_axisi; uint16_t i; - uint8_t count = 0; - for (i = 1; i < segments; i++) { // Increment (segments-1). + uint8_t count = 0; + float original_feedrate = pl_data->feed_rate; // Kinematics may alter the feedrate, so save an original copy + for (i = 1; i < segments; i++) { // Increment (segments-1). if (count < N_ARC_CORRECTION) { // Apply vector rotation matrix. ~40 usec r_axisi = r_axis0 * sin_T + r_axis1 * cos_T; @@ -206,14 +224,11 @@ void mc_arc(float* target, position[axis_0] = center_axis0 + r_axis0; position[axis_1] = center_axis1 + r_axis1; position[axis_linear] += linear_per_segment; -#ifdef USE_KINEMATICS - mc_line_kins(position, pl_data, previous_position); + pl_data->feed_rate = original_feedrate; // This restores the feedrate kinematics may have altered + cartesian_to_motors(position, pl_data, previous_position); previous_position[axis_0] = position[axis_0]; previous_position[axis_1] = position[axis_1]; previous_position[axis_linear] = position[axis_linear]; -#else - mc_line(position, pl_data); -#endif // Bail mid-circle on system abort. Runtime command check already performed by mc_line. if (sys.abort) { return; @@ -221,16 +236,16 @@ void mc_arc(float* target, } } // Ensure last segment arrives at target location. - mc_line_kins(target, pl_data, previous_position); + cartesian_to_motors(target, pl_data, previous_position); } // Execute dwell in seconds. -void mc_dwell(float seconds) { - if (sys.state == State::CheckMode) { - return; +bool mc_dwell(int32_t milliseconds) { + if (milliseconds <= 0 || sys.state == State::CheckMode) { + return false; } protocol_buffer_synchronize(); - delay_sec(seconds, DELAY_MODE_DWELL); + return delay_msec(milliseconds, DwellMode::Dwell); } // return true if the mask has exactly one bit set, @@ -247,16 +262,17 @@ static bool mask_is_single_axis(uint8_t axis_mask) { return axis_mask && ((axis_mask & (axis_mask - 1)) == 0); } -// return true if the axis is defined as a squared axis -// Squaring: is used on gantry type axes that have two motors -// Each motor with touch off its own switch to square the axis -static bool mask_has_squared_axis(uint8_t axis_mask) { - return axis_mask & homing_squared_axes->get(); -} - -// return true if axis_mask refers to a single squared axis static bool axis_is_squared(uint8_t axis_mask) { - return mask_is_single_axis(axis_mask) && mask_has_squared_axis(axis_mask); + // Squaring can only be done if it is the only axis in the mask + if (axis_mask & homing_squared_axes->get()) { + if (mask_is_single_axis(axis_mask)) { + return true; + } + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Cannot multi-axis home with squared axes. Homing normally"); + return false; + } + + return false; } #ifdef USE_I2S_STEPS @@ -283,18 +299,16 @@ static bool axis_is_squared(uint8_t axis_mask) { // executing the homing cycle. This prevents incorrect buffered plans after homing. void mc_homing_cycle(uint8_t cycle_mask) { bool no_cycles_defined = true; -#ifdef USE_CUSTOM_HOMING + if (user_defined_homing(cycle_mask)) { return; } -#endif + // This give kinematics a chance to do something before normal homing // if it returns true, the homing is canceled. -#ifdef USE_KINEMATICS if (kinematics_pre_homing(cycle_mask)) { return; } -#endif // Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems // with machines with limits wired on both ends of travel to one limit pin. // TODO: Move the pin-specific LIMIT_PIN call to Limits.cpp as a function. @@ -364,10 +378,8 @@ void mc_homing_cycle(uint8_t cycle_mask) { // Sync gcode parser and planner positions to homed position. gc_sync_position(); plan_sync_position(); -#ifdef USE_KINEMATICS // This give kinematics a chance to do something after normal homing kinematics_post_homing(); -#endif // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle. limits_init(); } @@ -438,7 +450,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully. } sys_probe_state = ProbeState::Off; // Ensure probe state monitor is disabled. - protocol_execute_realtime(); // Check and execute run-time commands + protocol_execute_realtime(); // Check and execute run-time commands // Reset the stepper and planner buffers to remove the remainder of the probe motion. st_reset(); // Reset step segment buffer. plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared. @@ -497,6 +509,7 @@ void mc_override_ctrl_update(uint8_t override_state) { // lost, since there was an abrupt uncontrolled deceleration. Called at an interrupt level by // realtime abort command and hard limits. So, keep to a minimum. void mc_reset() { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Debug, "mc_reset()"); // Only this function can set the system reset. Helps prevent multiple kill calls. if (!sys_rt_exec_state.bit.reset) { sys_rt_exec_state.bit.reset = true; @@ -506,11 +519,11 @@ void mc_reset() { MachineConfig::instance()->_coolant->stop(); // turn off all User I/O immediately - sys_io_control(0xFF, LOW, false); - sys_pwm_control(0xFF, 0, false); + sys_digital_all_off(); + sys_analog_all_off(); #ifdef ENABLE_SD_CARD // do we need to stop a running SD job? - if (get_sd_state(false) == SDCARD_BUSY_PRINTING) { + if (get_sd_state(false) == SDState::BusyPrinting) { //Report print stopped report_feedback_message(Message::SdFileQuit); closeFile(); diff --git a/Grbl_Esp32/src/MotionControl.h b/Grbl_Esp32/src/MotionControl.h index c9177f0b..c9f26c04 100644 --- a/Grbl_Esp32/src/MotionControl.h +++ b/Grbl_Esp32/src/MotionControl.h @@ -35,8 +35,8 @@ const int PARKING_MOTION_LINE_NUMBER = 0; // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. -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); +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position); +bool mc_line(float* target, plan_line_data_t* pl_data); // returns true if line was submitted to planner // Execute an arc in offset mode format. position == current xyz, target == target xyz, // offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is @@ -53,7 +53,7 @@ void mc_arc(float* target, uint8_t is_clockwise_arc); // Dwell for a specific number of seconds -void mc_dwell(float seconds); +bool mc_dwell(int32_t milliseconds); // Perform homing cycle to locate machine zero. Requires limit switches. void mc_homing_cycle(uint8_t cycle_mask); diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.cpp b/Grbl_Esp32/src/Motors/Dynamixel2.cpp index 3f5c5aa5..c14fee82 100644 --- a/Grbl_Esp32/src/Motors/Dynamixel2.cpp +++ b/Grbl_Esp32/src/Motors/Dynamixel2.cpp @@ -32,7 +32,7 @@ namespace Motors { uint8_t Motors::Dynamixel2::ids[MAX_N_AXIS][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } }; void Dynamixel2::init() { - _has_errors = false; // let's start with the assumption we're good. + _has_errors = false; // let's start with the assumption we're good. _axis_index = axis_index(); init_uart(_id, axis_index(), dual_axis_index()); // static and only allows one init @@ -197,10 +197,9 @@ namespace Motors { if (_has_errors) { return false; } - - auto axis = MachineConfig::instance()->_axes->_axis[_axis_index]; - sys_position[_axis_index] = - axis->_homing->_mpos * axis->_stepsPerMm; // convert to steps + + auto axis = MachineConfig::instance()->_axes->_axis[_axis_index]; + sys_position[_axis_index] = axis->_homing->_mpos * axis->_stepsPerMm; // convert to steps set_disable(false); set_location(); // force the PWM to update now @@ -354,16 +353,14 @@ namespace Motors { tx_message[++msg_index] = 4; // low order data length tx_message[++msg_index] = 0; // high order data length - auto n_axis = MachineConfig::instance()->_axes->_numberAxis; + auto n_axis = MachineConfig::instance()->_axes->_numberAxis; + float* mpos = system_get_mpos(); for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { current_id = ids[axis][gang_index]; if (current_id != 0) { count++; // keep track of the count for the message length - //determine the location of the axis - float target = system_convert_axis_steps_to_mpos(sys_position, axis); // get the axis machine position in mm - dxl_count_min = DXL_COUNT_MIN; dxl_count_max = DXL_COUNT_MAX; @@ -373,7 +370,7 @@ namespace Motors { // map the mm range to the servo range dxl_position = - (uint32_t)mapConstrain(target, limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max); + +(uint32_t)mapConstrain(mpos[axis], limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max); tx_message[++msg_index] = current_id; // ID of the servo tx_message[++msg_index] = dxl_position & 0xFF; // data @@ -452,9 +449,7 @@ namespace Motors { } // Configuration registration - namespace - { + namespace { MotorFactory::InstanceBuilder registration("dynamixel2"); } } - diff --git a/Grbl_Esp32/src/Motors/Motor.cpp b/Grbl_Esp32/src/Motors/Motor.cpp index c7e5f7ba..2bc8b678 100644 --- a/Grbl_Esp32/src/Motors/Motor.cpp +++ b/Grbl_Esp32/src/Motors/Motor.cpp @@ -16,7 +16,6 @@ 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 @@ -36,17 +35,18 @@ namespace Motors { void Motor::debug_message() {} + Motor::Motor(uint8_t axis_index) : _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {} bool Motor::test() { return true; }; // true = OK uint8_t Motor::axis_index() const { - Assert(MachineConfig::instance() != nullptr && - MachineConfig::instance()->_axes != nullptr, "Expected machine to be configured before this is called."); + Assert(MachineConfig::instance() != nullptr && MachineConfig::instance()->_axes != nullptr, + "Expected machine to be configured before this is called."); return MachineConfig::instance()->_axes->findAxisIndex(this); } uint8_t Motor::dual_axis_index() const { - Assert(MachineConfig::instance() != nullptr && - MachineConfig::instance()->_axes != nullptr, "Expected machine to be configured before this is called."); + Assert(MachineConfig::instance() != nullptr && MachineConfig::instance()->_axes != nullptr, + "Expected machine to be configured before this is called."); return MachineConfig::instance()->_axes->findAxisGanged(this); } diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp index 34cb6e04..2c273b2f 100644 --- a/Grbl_Esp32/src/Motors/RcServo.cpp +++ b/Grbl_Esp32/src/Motors/RcServo.cpp @@ -39,12 +39,26 @@ namespace Motors { auto pwmNative = _pwm_pin.getNative(Pin::Capabilities::PWM); +#ifdef LATER + char* setting_cal_min = (char*)malloc(20); + sprintf(setting_cal_min, "%c/RcServo/Cal/Min", report_get_axis_letter(_axis_index)); // + rc_servo_cal_min = new FloatSetting(EXTENDED, WG, NULL, setting_cal_min, 1.0, 0.5, 2.0); + + char* setting_cal_max = (char*)malloc(20); + sprintf(setting_cal_max, "%c/RcServo/Cal/Max", report_get_axis_letter(_axis_index)); // + rc_servo_cal_max = new FloatSetting(EXTENDED, WG, NULL, setting_cal_max, 1.0, 0.5, 2.0); + + rc_servo_cal_min->load(); + rc_servo_cal_max->load(); +#endif + read_settings(); _channel_num = sys_get_next_PWM_chan_num(); ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS); ledcAttachPin(pwmNative, _channel_num); _current_pwm_duty = 0; + _disabled = true; config_message(); startUpdateTask(); } @@ -72,10 +86,6 @@ namespace Motors { // sets the PWM to zero. This allows most servos to be manually moved void RcServo::set_disable(bool disable) { - if (_disabled == disable) { - return; - } - _disabled = disable; if (_disabled) { _write_pwm(0); @@ -102,11 +112,6 @@ namespace Motors { return; } - if (sys.state == State::Alarm) { - set_disable(true); - return; - } - read_settings(); mpos = system_convert_axis_steps_to_mpos(sys_position, _axis_index); // get the axis machine position in mm @@ -122,11 +127,14 @@ namespace Motors { } void RcServo::read_settings() { - _pwm_pulse_min = SERVO_MIN_PULSE * _cal_min; - _pwm_pulse_max = SERVO_MAX_PULSE * _cal_max; + if (bitnum_istrue(dir_invert_mask->get(), _axis_index)) { + // swap the pwm values + _pwm_pulse_min = SERVO_MAX_PULSE * (1.0 + (1.0 - rc_servo_cal_min->get())); + _pwm_pulse_max = SERVO_MIN_PULSE * (1.0 + (1.0 - rc_servo_cal_max->get())); - if (bitnum_istrue(dir_invert_mask->get(), _axis_index)) { // normal direction - swap(_pwm_pulse_min, _pwm_pulse_max); + } else { + _pwm_pulse_min = SERVO_MIN_PULSE * rc_servo_cal_min->get(); + _pwm_pulse_max = SERVO_MAX_PULSE * rc_servo_cal_max->get(); } } diff --git a/Grbl_Esp32/src/Motors/Servo.cpp b/Grbl_Esp32/src/Motors/Servo.cpp index 233d3206..5358a543 100644 --- a/Grbl_Esp32/src/Motors/Servo.cpp +++ b/Grbl_Esp32/src/Motors/Servo.cpp @@ -41,6 +41,7 @@ namespace Motors { } void Servo::startUpdateTask() { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Servo Update Task Started"); if (this == List) { xTaskCreatePinnedToCore(updateTask, // task "servoUpdateTask", // name for task @@ -48,7 +49,7 @@ namespace Motors { NULL, // parameters 1, // priority NULL, // handle - 0 // core + SUPPORT_TASK_CORE // core ); } } @@ -68,7 +69,9 @@ namespace Motors { vTaskDelayUntil(&xLastWakeTime, xUpdate); static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +#endif } } } diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp index aa618fab..afc0865c 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.cpp +++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp @@ -40,12 +40,13 @@ namespace Motors { } void StandardStepper::init() { - init_step_dir_pins(); + read_settings(); config_message(); } + void StandardStepper::read_settings() { init_step_dir_pins(); } void StandardStepper::init_step_dir_pins() { - auto axisIndex = axis_index(); + auto axisIndex = axis_index(); _invert_step_pin = bitnum_istrue(step_invert_mask->get(), axisIndex); _invert_dir_pin = bitnum_istrue(dir_invert_mask->get(), axisIndex); _dir_pin.setAttr(Pin::Attr::Output); @@ -61,11 +62,8 @@ namespace Motors { rmtConfig.tx_config.carrier_level = RMT_CARRIER_LEVEL_LOW; rmtConfig.tx_config.idle_output_en = true; -# ifdef STEP_PULSE_DELAY - rmtItem[0].duration0 = STEP_PULSE_DELAY * 4; -# else - rmtItem[0].duration0 = 1; -# endif + auto stepPulseDelay = direction_delay_microseconds->get(); + rmtItem[0].duration0 = stepPulseDelay < 1 ? 1 : stepPulseDelay * 4; rmtItem[0].duration1 = 4 * pulse_microseconds->get(); rmtItem[1].duration0 = 0; @@ -125,8 +123,7 @@ namespace Motors { void StandardStepper::set_disable(bool disable) { _disable_pin.write(disable); } // Configuration registration - namespace - { + namespace { MotorFactory::InstanceBuilder registration("standard_stepper"); } } diff --git a/Grbl_Esp32/src/Motors/StandardStepper.h b/Grbl_Esp32/src/Motors/StandardStepper.h index ae0a9fa5..edb06a1b 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.h +++ b/Grbl_Esp32/src/Motors/StandardStepper.h @@ -18,6 +18,7 @@ namespace Motors { void set_direction(bool) override; void step() override; void unstep() override; + void read_settings() override; void init_step_dir_pins(); diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index 108d9556..916290be 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -65,7 +65,6 @@ namespace Motors { } _has_errors = false; - init_step_dir_pins(); // from StandardStepper _cs_pin.setAttr(Pin::Attr::Output | Pin::Attr::InitialOn); @@ -115,7 +114,7 @@ namespace Motors { NULL, // parameters 1, // priority NULL, - 0 // core + SUPPORT_TASK_CORE // must run the task on same core ); } } else { @@ -216,6 +215,8 @@ namespace Motors { tmcstepper->microsteps(_microsteps); tmcstepper->rms_current(run_i_ma, hold_i_percent); + + init_step_dir_pins(); } bool TrinamicDriver::set_homing_mode(bool isHoming) { @@ -265,7 +266,7 @@ namespace Motors { tmcstepper->THIGH(calc_tstep(feedrate, 60.0)); tmcstepper->sfilt(1); tmcstepper->diag1_stall(true); // stallguard i/o is on diag1 - tmcstepper->sgt(_stallguard); + tmcstepper->sgt(constrain(axis_settings[_axis_index]->stallguard->get(), -64, 63)); break; } default: @@ -294,7 +295,7 @@ namespace Motors { tmcstepper->stallguard(), tmcstepper->sg_result(), feedrate, - _stallguard); + constrain(axis_settings[_axis_index]->stallguard->get(), -64, 63)); TMC2130_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. status.sr = tmcstepper->DRV_STATUS(); @@ -362,10 +363,6 @@ namespace Motors { xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. while (true) { // don't ever return from this or the task dies - if (motorSettingChanged) { - MachineConfig::instance()->_axes->read_settings(); - motorSettingChanged = false; - } if (stallguard_debug_mask->get() != 0) { if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { for (TrinamicDriver* p = List; p; p = p->link) { @@ -380,7 +377,9 @@ namespace Motors { vTaskDelayUntil(&xLastWakeTime, xreadSg); static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +#endif } } diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriver.h b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h new file mode 100644 index 00000000..440231a8 --- /dev/null +++ b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h @@ -0,0 +1,135 @@ +#pragma once + +/* + TrinamicUartDriver.h + + Part of Grbl_ESP32 + 2020 - The Ant Team + 2020 - Bart Dring + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include "Motor.h" +#include "StandardStepper.h" +#include "../Uart.h" + +#include // https://github.com/teemuatlut/TMCStepper + +const float TMC2208_RSENSE_DEFAULT = 0.11f; +const float TMC2209_RSENSE_DEFAULT = 0.11f; + +const double TRINAMIC_UART_FCLK = 12700000.0; // Internal clock Approx (Hz) used to calculate TSTEP from homing rate + +// ==== defaults OK to define them in your machine definition ==== + +#ifndef TRINAMIC_UART_RUN_MODE +# define TRINAMIC_UART_RUN_MODE TrinamicUartMode ::StealthChop +#endif + +#ifndef TRINAMIC_UART_HOMING_MODE +# define TRINAMIC_UART_HOMING_MODE TRINAMIC_UART_RUN_MODE +#endif + +#ifndef TRINAMIC_UART_TOFF_DISABLE +# define TRINAMIC_UART_TOFF_DISABLE 0 +#endif + +#ifndef TRINAMIC_UART_TOFF_STEALTHCHOP +# define TRINAMIC_UART_TOFF_STEALTHCHOP 5 +#endif + +#ifndef TRINAMIC_UART_TOFF_COOLSTEP +# define TRINAMIC_UART_TOFF_COOLSTEP 3 +#endif + +#ifndef TMC_UART +# define TMC_UART UART_NUM_2 +#endif + +#ifndef TMC_UART_RX +# define TMC_UART_RX UNDEFINED_PIN +#endif + +#ifndef TMC_UART_TX +# define TMC_UART_TX UNDEFINED_PIN +#endif + +extern Uart tmc_serial; + +namespace Motors { + + enum class TrinamicUartMode : uint8_t { + None = 0, // not for machine defs! + StealthChop = 1, + CoolStep = 2, + StallGuard = 3, + }; + + class TrinamicUartDriver : public StandardStepper { + private: + static bool _uart_started; + + public: + TrinamicUartDriver(uint8_t axis_index, + uint8_t step_pin, + uint8_t dir_pin, + uint8_t disable_pin, + uint16_t driver_part_number, + float r_senseS, + uint8_t address); + + void config_message(); + void hw_serial_init(); + void init(); + void set_mode(); + void read_settings(); + void debug_message(); + bool set_homing_mode(bool is_homing) override; + void set_disable(bool disable) override; + + uint8_t addr; + + private: + uint32_t calc_tstep(float speed, float percent); //TODO: see if this is useful/used. + + TMC2209Stepper* tmcstepper; // all other driver types are subclasses of this one + TrinamicUartMode _homing_mode; + uint16_t _driver_part_number; // example: use 2209 for TMC2209 + float _r_sense; + bool _has_errors; + bool _disabled; + + TrinamicUartMode _mode = TrinamicUartMode::None; + bool test(); + void set_mode(bool isHoming); + void trinamic_test_response(); + void trinamic_stepper_enable(bool enable); + + bool report_open_load(TMC2208_n ::DRV_STATUS_t status); + bool report_short_to_ground(TMC2208_n ::DRV_STATUS_t status); + bool report_over_temp(TMC2208_n ::DRV_STATUS_t status); + bool report_short_to_ps(TMC2208_n ::DRV_STATUS_t status); + + uint8_t get_next_index(); + + // Linked list of Trinamic driver instances, used by the + // StallGuard reporting task. TODO: verify if this is really used/useful. + static TrinamicUartDriver* List; + TrinamicUartDriver* link; + static void readSgTask(void*); + + protected: + // void config_message() override; + }; + +} diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp new file mode 100644 index 00000000..6409f1e6 --- /dev/null +++ b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp @@ -0,0 +1,401 @@ +/* + TrinamicUartDriverClass.cpp + + This is used for Trinamic UART controlled stepper motor drivers. + + Part of Grbl_ESP32 + 2020 - The Ant Team + 2020 - Bart Dring + + TMC2209 Datasheet + https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC2209_Datasheet_V103.pdf + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . + +*/ +#include "TrinamicUartDriver.h" + +#include + +Uart tmc_serial(TMC_UART); + +namespace Motors { + + bool TrinamicUartDriver::_uart_started = false; + + TrinamicUartDriver* TrinamicUartDriver::List = NULL; // a static ist of all drivers for stallguard reporting + + /* HW Serial Constructor. */ + TrinamicUartDriver::TrinamicUartDriver( + uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin, uint16_t driver_part_number, float r_sense, uint8_t addr) : + StandardStepper(axis_index, step_pin, dir_pin, disable_pin) { + _driver_part_number = driver_part_number; + _has_errors = false; + _r_sense = r_sense; + this->addr = addr; + + if (!_uart_started) { + tmc_serial.setPins(TMC_UART_TX, TMC_UART_RX); + tmc_serial.begin(115200, Uart::Data::Bits8, Uart::Stop::Bits1, Uart::Parity::None); + _uart_started = true; + } + hw_serial_init(); + + link = List; + List = this; + } + + void TrinamicUartDriver::hw_serial_init() { + if (_driver_part_number == 2209) + tmcstepper = new TMC2209Stepper(&tmc_serial, _r_sense, addr); + else { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unsupported Trinamic motor p/n:%d", _driver_part_number); + return; + } + } + + void TrinamicUartDriver ::init() { + if (_has_errors) { + return; + } + + init_step_dir_pins(); // from StandardStepper + config_message(); + + tmcstepper->begin(); + + _has_errors = !test(); // Try communicating with motor. Prints an error if there is a problem. + + /* If communication with the driver is working, read the + main settings, apply new driver settings and then read + them back. */ + if (!_has_errors) { //TODO: verify if this is the right way to set the Irun/IHold and microsteps. + read_settings(); + set_mode(false); + } + + // After initializing all of the TMC drivers, create a task to + // display StallGuard data. List == this for the final instance. + if (List == this) { + xTaskCreatePinnedToCore(readSgTask, // task + "readSgTask", // name for task + 4096, // size of task stack + NULL, // parameters + 1, // priority + NULL, + SUPPORT_TASK_CORE // must run the task on same core + // core + ); + } + } + + /* + This is the startup message showing the basic definition. + */ + void TrinamicUartDriver::config_message() { //TODO: The RX/TX pin could be added to the msg. + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s motor Trinamic TMC%d Step:%s Dir:%s Disable:%s UART%d Rx:%s Tx:%s Addr:%d R:%0.3f %s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + _driver_part_number, + pinName(_step_pin).c_str(), + pinName(_dir_pin).c_str(), + pinName(_disable_pin).c_str(), + TMC_UART, + pinName(TMC_UART_RX), + pinName(TMC_UART_TX), + this->addr, + _r_sense, + reportAxisLimitsMsg(_axis_index)); + } + + bool TrinamicUartDriver::test() { + if (_has_errors) { + return false; + } + switch (tmcstepper->test_connection()) { + case 1: + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s driver test failed. Check connection", + reportAxisNameMsg(_axis_index, _dual_axis_index)); + return false; + case 2: + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s driver test failed. Check motor power", + reportAxisNameMsg(_axis_index, _dual_axis_index)); + return false; + default: + // driver responded, so check for other errors from the DRV_STATUS register + + TMC2208_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. + status.sr = tmcstepper->DRV_STATUS(); + + bool err = false; + + // look for errors + if (report_short_to_ground(status)) { + err = true; + } + + if (report_over_temp(status)) { + err = true; + } + + if (report_short_to_ps(status)) { + err = true; + } + + if (err) { + return false; + } + + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s driver test passed", reportAxisNameMsg(_axis_index, _dual_axis_index)); + return true; + } + } + + /* + Read setting and send them to the driver. Called at init() and whenever related settings change + both are stored as float Amps, but TMCStepper library expects... + uint16_t run (mA) + float hold (as a percentage of run) + */ + void TrinamicUartDriver::read_settings() { + if (_has_errors) { + return; + } + + uint16_t run_i_ma = (uint16_t)(axis_settings[_axis_index]->run_current->get() * 1000.0); + float hold_i_percent; + + if (axis_settings[_axis_index]->run_current->get() == 0) + hold_i_percent = 0; + else { + hold_i_percent = axis_settings[_axis_index]->hold_current->get() / axis_settings[_axis_index]->run_current->get(); + if (hold_i_percent > 1.0) + hold_i_percent = 1.0; + } + tmcstepper->microsteps(axis_settings[_axis_index]->microsteps->get()); + tmcstepper->rms_current(run_i_ma, hold_i_percent); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "Setting current of driver %s, target: %u, read irun: %d, hold percent: %f, usteps: %d", + // reportAxisNameMsg(_axis_index, _dual_axis_index), run_i_ma, tmcstepper->rms_current(), hold_i_percent, axis_settings[_axis_index]->microsteps->get()); + } + + bool TrinamicUartDriver::set_homing_mode(bool isHoming) { + set_mode(isHoming); + return true; + } + + /* + There are ton of settings. I'll start by grouping then into modes for now. + Many people will want quiet and stallgaurd homing. Stallguard only run in + Coolstep mode, so it will need to switch to Coolstep when homing + */ + void TrinamicUartDriver::set_mode(bool isHoming) { + if (_has_errors) { + return; + } + + TrinamicUartMode newMode = isHoming ? TRINAMIC_UART_HOMING_MODE : TRINAMIC_UART_RUN_MODE; + + if (newMode == _mode) { + return; + } + + _mode = newMode; + + switch (_mode) { + case TrinamicUartMode ::StealthChop: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "StealthChop"); + tmcstepper->en_spreadCycle(false); + tmcstepper->pwm_autoscale(true); + break; + case TrinamicUartMode ::CoolStep: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep"); + // tmcstepper->en_pwm_mode(false); //TODO: check if this is present in TMC2208/09 + tmcstepper->en_spreadCycle(true); + tmcstepper->pwm_autoscale(false); + break; + case TrinamicUartMode ::StallGuard: //TODO: check all configurations for stallguard + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard"); + tmcstepper->en_spreadCycle(false); + tmcstepper->pwm_autoscale(false); + tmcstepper->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); + tmcstepper->SGTHRS(constrain(axis_settings[_axis_index]->stallguard->get(), 0, 255)); + break; + default: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unknown Trinamic mode:d", _mode); + } + } + + /* + This is the stallguard tuning info. It is call debug, so it could be generic across all classes. + */ + void TrinamicUartDriver::debug_message() { + if (_has_errors) { + return; + } + uint32_t tstep = tmcstepper->TSTEP(); + + if (tstep == 0xFFFFF || tstep < 1) { // if axis is not moving return + return; + } + float feedrate = st_get_realtime_rate(); //* settings.microsteps[axis_index] / 60.0 ; // convert mm/min to Hz + + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s SG_Val: %04d Rate: %05.0f mm/min SG_Setting:%d", + reportAxisNameMsg(_axis_index, _dual_axis_index), + tmcstepper->SG_RESULT(), // tmcstepper->sg_result(), + feedrate, + axis_settings[_axis_index]->stallguard->get()); + + TMC2208_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. + status.sr = tmcstepper->DRV_STATUS(); + + // these only report if there is a fault condition + report_open_load(status); + report_short_to_ground(status); + report_over_temp(status); + report_short_to_ps(status); + } + + // calculate a tstep from a rate + // tstep = TRINAMIC_UART_FCLK / (time between 1/256 steps) + // This is used to set the stallguard window from the homing speed. + // The percent is the offset on the window + uint32_t TrinamicUartDriver::calc_tstep(float speed, float percent) { + float tstep = + speed / 60.0 * axis_settings[_axis_index]->steps_per_mm->get() * (float)(256 / axis_settings[_axis_index]->microsteps->get()); + tstep = TRINAMIC_UART_FCLK / tstep * percent / 100.0; + + return static_cast(tstep); + } + + // this can use the enable feature over SPI. The dedicated pin must be in the enable mode, + // but that can be hardwired that way. + void TrinamicUartDriver::set_disable(bool disable) { + if (_has_errors) { + return; + } + + if (_disabled == disable) { + return; + } + + _disabled = disable; + + digitalWrite(_disable_pin, _disabled); + +#ifdef USE_TRINAMIC_ENABLE + if (_disabled) { + tmcstepper->toff(TRINAMIC_UART_TOFF_DISABLE); + } else { + if (_mode == TrinamicUartMode::StealthChop) { + tmcstepper->toff(TRINAMIC_UART_TOFF_STEALTHCHOP); + } else { + tmcstepper->toff(TRINAMIC_UART_TOFF_COOLSTEP); + } + } +#endif + // the pin based enable could be added here. + // This would be for individual motors, not the single pin for all motors. + } + + // =========== Reporting functions ======================== + + bool TrinamicUartDriver::report_open_load(TMC2208_n ::DRV_STATUS_t status) { + if (status.ola || status.olb) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver open load A:%s B:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.ola ? "Y" : "N", + status.olb ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicUartDriver::report_short_to_ground(TMC2208_n ::DRV_STATUS_t status) { + if (status.s2ga || status.s2gb) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver shorted coil A:%s B:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.s2ga ? "Y" : "N", + status.s2gb ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicUartDriver::report_over_temp(TMC2208_n ::DRV_STATUS_t status) { + if (status.ot || status.otpw) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver temp Warning:%s Fault:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.otpw ? "Y" : "N", + status.ot ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicUartDriver::report_short_to_ps(TMC2208_n ::DRV_STATUS_t status) { + // check for short to power supply + if ((status.sr & bit(12)) || (status.sr & bit(13))) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver short vsa:%s vsb:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + (status.sr & bit(12)) ? "Y" : "N", + (status.sr & bit(13)) ? "Y" : "N"); + return true; + } + return false; // no error + } + + // Prints StallGuard data that is useful for tuning. + void TrinamicUartDriver::readSgTask(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xreadSg = 200; // in ticks (typically ms) + auto n_axis = number_axis->get(); + + xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. + while (true) { // don't ever return from this or the task dies + if (stallguard_debug_mask->get() != 0) { + if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { + for (TrinamicUartDriver* p = List; p; p = p->link) { + if (bitnum_istrue(stallguard_debug_mask->get(), p->_axis_index)) { + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", stallguard_debug_mask->get()); + p->debug_message(); + } + } + } // sys.state + } // if mask + + vTaskDelayUntil(&xLastWakeTime, xreadSg); + + static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK + reportTaskStackSize(uxHighWaterMark); +#endif + } + } +} diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp index 358a201a..a1b34fc6 100644 --- a/Grbl_Esp32/src/NutsBolts.cpp +++ b/Grbl_Esp32/src/NutsBolts.cpp @@ -114,23 +114,28 @@ void delay_ms(uint16_t ms) { } // Non-blocking delay function used for general operation and suspend features. -void delay_sec(float seconds, uint8_t mode) { - uint16_t i = ceil(1000 / DWELL_TIME_STEP * seconds); +bool delay_msec(int32_t milliseconds, DwellMode mode) { + // Note: i must be signed, because of the 'i-- > 0' check below. + int32_t i = milliseconds / DWELL_TIME_STEP; + int32_t remainder = i < 0 ? 0 : (milliseconds - DWELL_TIME_STEP * i); + while (i-- > 0) { if (sys.abort) { - return; + return false; } - if (mode == DELAY_MODE_DWELL) { + if (mode == DwellMode::Dwell) { protocol_execute_realtime(); - } else { // DELAY_MODE_SYS_SUSPEND + } else { // DwellMode::SysSuspend // Execute rt_system() only to avoid nesting suspend loops. protocol_exec_rt_system(); if (sys.suspend.bit.restartRetract) { - return; // Bail, if safety door reopens. + return false; // Bail, if safety door reopens. } } delay(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment } + delay(remainder); + return true; } // Simple hypotenuse computation function. diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h index 7a115f2a..3c65659e 100644 --- a/Grbl_Esp32/src/NutsBolts.h +++ b/Grbl_Esp32/src/NutsBolts.h @@ -27,6 +27,11 @@ #define UNDEFINED_PIN "" +enum class DwellMode : uint8_t { + Dwell = 0, // (Default: Must be zero) + SysSuspend = 1, //G92.1 (Do not alter value) +}; + const double SOME_LARGE_VALUE = 1.0E+38; // Axis array index values. Must start with 0 and be continuous. @@ -59,9 +64,6 @@ static inline int toMotor2(int axis) { const double MM_PER_INCH = (25.40); const double INCH_PER_MM = (0.0393701); -const int DELAY_MODE_DWELL = 0; -const int DELAY_MODE_SYS_SUSPEND = 1; - // Useful macros #define clear_vector(a) memset(a, 0, sizeof(a)) #define clear_vector_float(a) memset(a, 0.0, sizeof(float) * MAX_N_AXIS) @@ -89,7 +91,7 @@ const int DELAY_MODE_SYS_SUSPEND = 1; uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr); // Non-blocking delay function used for general operation and suspend features. -void delay_sec(float seconds, uint8_t mode); +bool delay_msec(int32_t milliseconds, DwellMode mode); // Delays variable-defined milliseconds. Compiler compatibility fix for _delay_ms(). void delay_ms(uint16_t ms); diff --git a/Grbl_Esp32/src/Planner.h b/Grbl_Esp32/src/Planner.h index 1f5f3eb4..812229c9 100644 --- a/Grbl_Esp32/src/Planner.h +++ b/Grbl_Esp32/src/Planner.h @@ -91,6 +91,7 @@ typedef struct { #ifdef USE_LINE_NUMBERS int32_t line_number; // Desired line number to report when executing. #endif + bool is_jog; // true if this was generated due to a jog command } plan_line_data_t; // Initialize and reset the motion plan subsystem diff --git a/Grbl_Esp32/src/Probe.cpp b/Grbl_Esp32/src/Probe.cpp index d371ade6..831e9a37 100644 --- a/Grbl_Esp32/src/Probe.cpp +++ b/Grbl_Esp32/src/Probe.cpp @@ -54,7 +54,7 @@ void Probe::set_direction(bool is_away) { // Returns the probe pin state. Triggered = true. Called by gcode parser and probe state monitor. bool Probe::get_state() { - return _probePin.read() ^ probe_invert->get(); + return _probePin.undefined() ? false : _probePin.read() ^ probe_invert->get(); } // Monitors probe pin state and records the system position when detected. Called by the @@ -70,7 +70,6 @@ void Probe::state_monitor() { void Probe::validate() const {} -void Probe::handle(Configuration::HandlerBase& handler) -{ +void Probe::handle(Configuration::HandlerBase& handler) { handler.handle("pin", _probePin); } diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index d77945b9..1e65d29c 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -1,5 +1,6 @@ #include "Grbl.h" #include +#include "Regex.h" // WG Readable and writable as guest // WU Readable and writable as user and admin @@ -54,12 +55,14 @@ void settings_restore(uint8_t restore_flag) { } } } + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Settings reset done"); } if (restore_flag & SettingsRestore::Parameters) { for (auto idx = CoordIndex::Begin; idx < CoordIndex::End; ++idx) { coords[idx]->setDefault(); } } + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Position offsets reset done"); } // Get settings values from non volatile storage into memory @@ -77,7 +80,6 @@ namespace WebUI { } void settings_init() { - EEPROM.begin(EEPROM_SIZE); make_settings(); WebUI::make_web_settings(); make_grbl_commands(); @@ -208,6 +210,7 @@ Error toggle_check_mode(const char* value, WebUI::AuthenticationLevel auth_level // is idle and ready, regardless of alarm locks. This is mainly to keep things // simple and consistent. if (sys.state == State::CheckMode) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Debug, "Check mode"); mc_reset(); report_feedback_message(Message::Disabled); } else { @@ -376,7 +379,6 @@ Error listAlarms(const char* value, WebUI::AuthenticationLevel auth_level, WebUI return Error::Ok; } - const char* errorString(Error errorNumber) { auto it = ErrorNames.find(errorNumber); return it == ErrorNames.end() ? NULL : it->second; @@ -406,17 +408,36 @@ Error listErrors(const char* value, WebUI::AuthenticationLevel auth_level, WebUI return Error::Ok; } -static bool anyState() { - return false; -} -static bool idleOrJog() { - return sys.state != State::Idle && sys.state != State::Jog; -} -static bool idleOrAlarm() { - return sys.state != State::Idle && sys.state != State::Alarm; -} -static bool notCycleOrHold() { - return sys.state == State::Cycle && sys.state == State::Hold; +Error motor_disable(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { + char* s; + if (value == NULL) { + value = "\0"; + } + + s = strdup(value); + s = trim(s); + + int32_t convertedValue; + char* endptr; + if (*s == '\0') { + convertedValue = 255; // all axes + } else { + convertedValue = strtol(s, &endptr, 10); + if (endptr == s || *endptr != '\0') { + // Try to convert as an axis list + convertedValue = 0; + auto axisNames = String("XYZABC"); + while (*s) { + int index = axisNames.indexOf(toupper(*s++)); + if (index < 0) { + return Error::BadNumberFormat; + } + convertedValue |= bit(index); + } + } + } + motors_set_disable(true, convertedValue); + return Error::Ok; } // Commands use the same syntax as Settings, but instead of setting or @@ -446,6 +467,8 @@ void make_grbl_commands() { new GrblCommand("V", "Settings/Stats", Setting::report_nvs_stats, idleOrAlarm); new GrblCommand("#", "GCode/Offsets", report_ngc, idleOrAlarm); new GrblCommand("H", "Home", home_all, idleOrAlarm); + new GrblCommand("MD", "Motor/Disable", motor_disable, idleOrAlarm); + #ifdef HOMING_SINGLE_AXIS_COMMANDS new GrblCommand("HX", "Home/X", home_x, idleOrAlarm); new GrblCommand("HY", "Home/Y", home_y, idleOrAlarm); @@ -551,19 +574,13 @@ Error do_command_or_setting(const char* key, char* value, WebUI::AuthenticationL Error retval = Error::InvalidStatement; if (!value) { auto lcKey = String(key); - // We allow the key string to begin with *, which we remove. - // This lets us look at X axis settings with $*x. - // $x by itself is the disable alarm lock command - if (lcKey.startsWith("*")) { - lcKey.remove(0, 1); - } lcKey.toLowerCase(); bool found = false; for (Setting* s = Setting::List; s; s = s->next()) { auto lcTest = String(s->getName()); lcTest.toLowerCase(); - if (lcTest.indexOf(lcKey) >= 0) { + if (regexMatch(lcKey.c_str(), lcTest.c_str())) { const char* displayValue = auth_failed(s, value, auth_level) ? "" : s->getStringValue(); show_setting(s->getName(), displayValue, NULL, out); found = true; diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index 2fb2b046..24de9325 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -5,8 +5,8 @@ Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC Copyright (c) 2009-2011 Simen Svale Skogsrud - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P + 2018 - Bart Dring This file was modifed for use on the ESP32 + CPU. Do not use this with Grbl for atMega328P Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -105,7 +105,7 @@ bool can_park() { GRBL PRIMARY LOOP: */ void protocol_main_loop() { - serial_reset_read_buffer(CLIENT_ALL); + client_reset_read_buffer(CLIENT_ALL); empty_lines(); //uint8_t client = CLIENT_SERIAL; // default client // Perform some machine checks to make sure everything is good to go. @@ -137,7 +137,7 @@ 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 c; + int c; for (;;) { #ifdef ENABLE_SD_CARD if (SD_ready_next) { @@ -159,7 +159,7 @@ void protocol_main_loop() { uint8_t client = CLIENT_SERIAL; char* line; for (client = 0; client < CLIENT_COUNT; client++) { - while ((c = serial_read(client)) != SERIAL_NO_DATA) { + while ((c = client_read(client)) != -1) { Error res = add_char_to_line(c, client); switch (res) { case Error::Ok: @@ -195,7 +195,7 @@ void protocol_main_loop() { return; // Bail to main() program loop to reset system. } // check to see if we should disable the stepper drivers ... esp32 work around for disable in main loop. - if (stepper_idle) { + if (stepper_idle && stepper_idle_lock_time->get() != 0xff) { if (esp_timer_get_time() > stepper_idle_counter) { MachineConfig::instance()->_axes->set_disable(true); } @@ -534,7 +534,6 @@ static void protocol_exec_rt_suspend() { #ifdef PARKING_ENABLE // Declare and initialize parking local variables float restore_target[MAX_N_AXIS]; - float parking_target[MAX_N_AXIS]; float retract_waypoint = PARKING_PULLOUT_INCREMENT; plan_line_data_t plan_data; plan_line_data_t* pl_data = &plan_data; @@ -569,12 +568,21 @@ static void protocol_exec_rt_suspend() { if (sys.abort) { return; } + // if a jogCancel comes in and we have a jog "in-flight" (parsed and handed over to mc_line()), + // then we need to cancel it before it reaches the planner. otherwise we may try to move way out of + // normal bounds, especially with senders that issue a series of jog commands before sending a cancel. + if (sys.suspend.bit.jogCancel && sys_pl_data_inflight != NULL && ((plan_line_data_t*)sys_pl_data_inflight)->is_jog) { + sys_pl_data_inflight = NULL; + } // Block until initial hold is complete and the machine has stopped motion. if (sys.suspend.bit.holdComplete) { // Parking manager. Handles de/re-energizing, switch state checks, and parking motions for // the safety door and sleep states. if (sys.state == State::SafetyDoor || sys.state == State::Sleep) { // Handles retraction motions and de-energizing. +#ifdef PARKING_ENABLE + float* parking_target = system_get_mpos(); +#endif if (!sys.suspend.bit.retractComplete) { // Ensure any prior spindle stop override is disabled at start of safety door routine. sys.spindle_stop_ovr.value = 0; // Disable override @@ -583,9 +591,8 @@ static void protocol_exec_rt_suspend() { MachineConfig::instance()->_coolant->off(); #else // Get current position and store restore location and spindle retract waypoint. - system_convert_array_steps_to_mpos(parking_target, sys_position); if (!sys.suspend.bit.restartRetract) { - memcpy(restore_target, parking_target, sizeof(parking_target)); + memcpy(restore_target, parking_target, sizeof(restore_target[0]) * number_axis->get()); retract_waypoint += restore_target[PARKING_AXIS]; retract_waypoint = MIN(retract_waypoint, PARKING_TARGET); } @@ -668,7 +675,7 @@ static void protocol_exec_rt_suspend() { sys.step_control.updateSpindleRpm = true; } else { spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed); - delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SYS_SUSPEND); + // restore delay is done in the spindle class } } } @@ -677,7 +684,7 @@ static void protocol_exec_rt_suspend() { if (!sys.suspend.bit.restartRetract) { // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin. MachineConfig::instance()->_coolant->set_state(restore_coolant); - delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND); + delay_msec(int32_t(1000.0 * coolant_start_delay->get()), DwellMode::SysSuspend); } } #ifdef PARKING_ENABLE diff --git a/Grbl_Esp32/src/Regex.cpp b/Grbl_Esp32/src/Regex.cpp new file mode 100644 index 00000000..7f942ccc --- /dev/null +++ b/Grbl_Esp32/src/Regex.cpp @@ -0,0 +1,60 @@ +// Simple regular expression matcher from Rob Pike per +// https://www.cs.princeton.edu/courses/archive/spr09/cos333/beautiful.html + +// c matches any literal character c +// ^ matches the beginning of the input string +// $ matches the end of the input string +// * matches zero or more occurrences of any character + +// The code therein has been reformatted into the style used in this +// project while replacing ints used as flags by bools. The regex +// syntax was changed by omitting '.' and making '*' equivalent to +// ".*". This regular expression matcher is for matching setting +// names, where arbitrary repetion of literal characters is +// unlikely. Literal character repetition is most useful for +// skipping whitespace, which does not occur in setting names. The +// "bare * wildcard" is similar to filename wildcarding in many shells +// and CLIs. + +static bool matchHere(const char* regexp, const char* text); + +// matchStar - search for *regexp at beginning of text +static bool matchStar(const char* regexp, const char* text) { + do { + if (matchHere(regexp, text)) { + return true; + } + } while (*text++ != '\0'); + return false; +} + +// matchHere - search for regex at beginning of text +static bool matchHere(const char* regexp, const char* text) { + if (regexp[0] == '\0') { + return true; + } + if (regexp[0] == '*') { + return matchStar(regexp + 1, text); + } + if (regexp[0] == '$' && regexp[1] == '\0') { + return *text == '\0'; + } + if (*text != '\0' && (regexp[0] == *text)) { + return matchHere(++regexp, ++text); + } + return false; +} + +// match - search for regular expression anywhere in text +// Returns true if text contains the regular expression regexp +bool regexMatch(const char* regexp, const char* text) { + if (regexp[0] == '^') { + return matchHere(++regexp, text); + } + do { + if (matchHere(regexp, text)) { + return true; + } + } while (*text++ != '\0'); + return false; +} diff --git a/Grbl_Esp32/src/Regex.h b/Grbl_Esp32/src/Regex.h new file mode 100644 index 00000000..9c0c5d13 --- /dev/null +++ b/Grbl_Esp32/src/Regex.h @@ -0,0 +1,5 @@ +// Simple regular expression matcher. +// See Regex.cpp for attribution, description and discussion + +// Returns true if text contains the regular expression regexp +bool regexMatch(const char* regexp, const char* text); diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 5eeabc2c..ab5205a4 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -57,35 +57,8 @@ const int DEFAULTBUFFERSIZE = 64; portMUX_TYPE mmux = portMUX_INITIALIZER_UNLOCKED; -// this is a generic send function that everything should use, so interfaces could be added (Bluetooth, etc) void grbl_send(uint8_t client, const char* text) { - if (client == CLIENT_INPUT) { - return; - } - - portENTER_CRITICAL(&mmux); - -#ifdef ENABLE_BLUETOOTH - if (WebUI::SerialBT.hasClient() && (client == CLIENT_BT || client == CLIENT_ALL)) { - WebUI::SerialBT.print(text); - //delay(10); // possible fix for dropped characters - } -#endif -#if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_OUT) - if (client == CLIENT_WEBUI || client == CLIENT_ALL) { - WebUI::Serial2Socket.write((const uint8_t*)text, strlen(text)); - } -#endif -#if defined(ENABLE_WIFI) && defined(ENABLE_TELNET) - if (client == CLIENT_TELNET || client == CLIENT_ALL) { - WebUI::telnet_server.write((const uint8_t*)text, strlen(text)); - } -#endif - if (client == CLIENT_SERIAL || client == CLIENT_ALL) { - Serial.print(text); - } - - portEXIT_CRITICAL(&mmux); + client_write(client, text); } // This is a formating version of the grbl_send(CLIENT_ALL,...) function that work like printf @@ -119,9 +92,13 @@ void grbl_msg_sendf(uint8_t client, MsgLevel level, const char* format, ...) { if (client == CLIENT_INPUT) { return; } - if (level > GRBL_MSG_LEVEL) { - return; + + if (message_level != NULL) { // might be null before messages are setup + if (level > static_cast(message_level->get())) { + return; + } } + char loc_buf[100]; char* temp = loc_buf; va_list arg; @@ -230,7 +207,7 @@ void report_status_message(Error status_code, uint8_t client) { switch (status_code) { case Error::Ok: // Error::Ok #ifdef ENABLE_SD_CARD - if (get_sd_state(false) == SDCARD_BUSY_PRINTING) { + if (get_sd_state(false) == SDState::BusyPrinting) { SD_ready_next = true; // flag so system_execute_line() will send the next line } else { grbl_send(client, "ok\r\n"); @@ -242,11 +219,12 @@ void report_status_message(Error status_code, uint8_t client) { default: #ifdef ENABLE_SD_CARD // do we need to stop a running SD job? - if (get_sd_state(false) == SDCARD_BUSY_PRINTING) { + if (get_sd_state(false) == SDState::BusyPrinting) { if (status_code == Error::GcodeUnsupportedCommand) { grbl_sendf(client, "error:%d\r\n", status_code); // most senders seem to tolerate this error and keep on going grbl_sendf(CLIENT_ALL, "error:%d in SD file at line %d\r\n", status_code, sd_get_current_line_number()); // don't close file + SD_ready_next = true; // flag so system_execute_line() will send the next line } else { grbl_notifyf("SD print error", "Error:%d during SD file at line: %d", status_code, sd_get_current_line_number()); grbl_sendf(CLIENT_ALL, "error:%d in SD file at line %d\r\n", status_code, sd_get_current_line_number()); @@ -294,10 +272,14 @@ std::map MessageText = { // NOTE: For interfaces, messages are always placed within brackets. And if silent mode // is installed, the message number codes are less than zero. void report_feedback_message(Message message) { // ok to send to all clients +#if defined(ENABLE_SD_CARD) if (message == Message::SdFileQuit) { grbl_notifyf("SD print canceled", "Reset during SD file at line: %d", sd_get_current_line_number()); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Reset during SD file at line: %d", sd_get_current_line_number()); - } else { + + } else +#endif //ENABLE_SD_CARD + { auto it = MessageText.find(message); if (it != MessageText.end()) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, it->second); @@ -320,11 +302,11 @@ void report_grbl_help(uint8_t client) { // These values are retained until Grbl is power-cycled, whereby they will be re-zeroed. void report_probe_parameters(uint8_t client) { // Report in terms of machine position. - float print_position[MAX_N_AXIS]; - char probe_rpt[(axesStringLen + 13 + 6 + 1)]; // the probe report we are building here - char temp[axesStringLen]; + char probe_rpt[(axesStringLen + 13 + 6 + 1)]; // the probe report we are building here + char temp[axesStringLen]; strcpy(probe_rpt, "[PRB:"); // initialize the string with the first characters // get the machine position and put them into a string and append to the probe report + float print_position[MAX_N_AXIS]; system_convert_array_steps_to_mpos(print_position, sys_probe_position); report_util_axis_values(print_position, temp); strcat(probe_rpt, temp); @@ -591,85 +573,30 @@ void report_echo_line_received(char* line, uint8_t client) { grbl_sendf(client, "[echo: %s]\r\n", line); } +// Calculate the position for status reports. +// float print_position = returned position +// float wco = returns the work coordinate offset +// bool wpos = true for work position compensation + // Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram // and the actual location of the CNC machine. Users may change the following function to their // specific needs, but the desired real-time data report must be as short as possible. This is // requires as it minimizes the computational overhead and allows grbl to keep running smoothly, // especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz). void report_realtime_status(uint8_t client) { - uint8_t idx; - int32_t current_position[MAX_N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - float print_position[MAX_N_AXIS]; - char status[200]; - char temp[MAX_N_AXIS * 20]; - system_convert_array_steps_to_mpos(print_position, current_position); - // Report current machine state and sub-states + char status[200]; + char temp[MAX_N_AXIS * 20]; + strcpy(status, "<"); - switch (sys.state) { - case State::Idle: - strcat(status, "Idle"); - break; - case State::Cycle: - strcat(status, "Run"); - break; - case State::Hold: - if (!(sys.suspend.bit.jogCancel)) { - strcat(status, "Hold:"); - strcat(status, sys.suspend.bit.holdComplete ? "0" : "1"); // Ready to resume - break; - } // Continues to print jog state during jog cancel. - case State::Jog: - strcat(status, "Jog"); - break; - case State::Homing: - strcat(status, "Home"); - break; - case State::Alarm: - strcat(status, "Alarm"); - break; - case State::CheckMode: - strcat(status, "Check"); - break; - case State::SafetyDoor: - strcat(status, "Door:"); - if (sys.suspend.bit.initiateRestore) { - strcat(status, "3"); // Restoring - } else { - if (sys.suspend.bit.retractComplete) { - strcat(status, sys.suspend.bit.safetyDoorAjar ? "1" : "0"); // Door ajar - // Door closed and ready to resume - } else { - strcat(status, "2"); // Retracting - } - } - break; - case State::Sleep: - strcat(status, "Sleep"); - break; - } - float wco[MAX_N_AXIS]; - if (bit_isfalse(status_mask->get(), RtStatus::Position) || (sys.report_wco_counter == 0)) { - auto n_axis = MachineConfig::instance()->_axes->_numberAxis; - 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(status_mask->get(), RtStatus::Position)) { - print_position[idx] -= wco[idx]; - } - } - } - // Report machine position + strcat(status, report_state_text()); + + // Report position + float* print_position = system_get_mpos(); if (bit_istrue(status_mask->get(), RtStatus::Position)) { strcat(status, "|MPos:"); } else { -#ifdef USE_FWD_KINEMATICS - forward_kinematics(print_position); -#endif strcat(status, "|WPos:"); + mpos_to_wpos(print_position); } report_util_axis_values(print_position, temp); strcat(status, temp); @@ -689,7 +616,7 @@ void report_realtime_status(uint8_t client) { } # endif //ENABLE_BLUETOOTH if (client == CLIENT_SERIAL) { - bufsize = serial_get_rx_buffer_available(CLIENT_SERIAL); + bufsize = client_get_rx_buffer_available(CLIENT_SERIAL); } sprintf(temp, "|Bf:%d,%d", plan_get_block_buffer_available(), bufsize); strcat(status, temp); @@ -761,16 +688,16 @@ void report_realtime_status(uint8_t client) { strcat(status, "S"); } if (ctrl_pin_state.bit.macro0) { - strcat(status, "M0"); + strcat(status, "0"); } if (ctrl_pin_state.bit.macro1) { - strcat(status, "M1"); + strcat(status, "1"); } if (ctrl_pin_state.bit.macro2) { - strcat(status, "M2"); + strcat(status, "2"); } if (ctrl_pin_state.bit.macro3) { - strcat(status, "M3"); + strcat(status, "3"); } } } @@ -794,7 +721,7 @@ void report_realtime_status(uint8_t client) { sys.report_ovr_counter = 1; // Set override on next report. } strcat(status, "|WCO:"); - report_util_axis_values(wco, temp); + report_util_axis_values(get_wco(), temp); strcat(status, temp); } #endif @@ -846,7 +773,7 @@ void report_realtime_status(uint8_t client) { } #endif #ifdef ENABLE_SD_CARD - if (get_sd_state(false) == SDCARD_BUSY_PRINTING) { + if (get_sd_state(false) == SDState::BusyPrinting) { sprintf(temp, "|SD:%4.2f,", sd_report_perc_complete()); strcat(status, temp); sd_get_current_filename(temp); @@ -916,6 +843,54 @@ void report_hex_msg(uint8_t* buf, const char* prefix, int len) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s", report); } +char* report_state_text() { + static char state[10]; + + switch (sys.state) { + case State::Idle: + strcpy(state, "Idle"); + break; + case State::Cycle: + strcpy(state, "Run"); + break; + case State::Hold: + if (!(sys.suspend.bit.jogCancel)) { + sys.suspend.bit.holdComplete ? strcpy(state, "Hold:0") : strcpy(state, "Hold:1"); + break; + } // Continues to print jog state during jog cancel. + case State::Jog: + strcpy(state, "Jog"); + break; + case State::Homing: + strcpy(state, "Home"); + break; + case State::Alarm: + strcpy(state, "Alarm"); + break; + case State::CheckMode: + strcpy(state, "Check"); + break; + case State::SafetyDoor: + strcpy(state, "Door:"); + if (sys.suspend.bit.initiateRestore) { + strcat(state, "3"); // Restoring + } else { + if (sys.suspend.bit.retractComplete) { + sys.suspend.bit.safetyDoorAjar ? strcat(state, "1") : strcat(state, "0"); + ; // Door ajar + // Door closed and ready to resume + } else { + strcat(state, "2"); // Retracting + } + } + break; + case State::Sleep: + strcpy(state, "Sleep"); + break; + } + return state; +} + char report_get_axis_letter(uint8_t axis) { switch (axis) { case X_AXIS: @@ -962,3 +937,24 @@ void reportTaskStackSize(UBaseType_t& saved) { } #endif } + +void mpos_to_wpos(float* position) { + float* wco = get_wco(); + auto n_axis = number_axis->get(); + for (int idx = 0; idx < n_axis; idx++) { + position[idx] -= wco[idx]; + } +} + +float* get_wco() { + static float wco[MAX_N_AXIS]; + auto n_axis = number_axis->get(); + for (int 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; + } + } + return wco; +} diff --git a/Grbl_Esp32/src/Report.h b/Grbl_Esp32/src/Report.h index de383920..cf7cb6c8 100644 --- a/Grbl_Esp32/src/Report.h +++ b/Grbl_Esp32/src/Report.h @@ -54,8 +54,8 @@ enum class Message : uint8_t { #define CLIENT_ALL 0xFF #define CLIENT_COUNT 5 // total number of client types regardless if they are used -enum class MsgLevel : uint8_t { - None = 0, // set GRBL_MSG_LEVEL in config.h to the level you want to see +enum class MsgLevel : int8_t { // Use $Message/Level + None = 0, Error = 1, Warning = 2, Info = 3, @@ -124,10 +124,13 @@ void report_machine_type(uint8_t client); void report_hex_msg(char* buf, const char* prefix, int len); void report_hex_msg(uint8_t* buf, const char* prefix, int len); -char report_get_axis_letter(uint8_t axis); - +char report_get_axis_letter(uint8_t axis); char* reportAxisLimitsMsg(uint8_t axis); char* reportAxisNameMsg(uint8_t axis); char* reportAxisNameMsg(uint8_t axis, uint8_t dual_axis); void reportTaskStackSize(UBaseType_t& saved); + +char* report_state_text(); +float* get_wco(); +void mpos_to_wpos(float* position); diff --git a/Grbl_Esp32/src/SDCard.cpp b/Grbl_Esp32/src/SDCard.cpp index 02da8148..3f0858cd 100644 --- a/Grbl_Esp32/src/SDCard.cpp +++ b/Grbl_Esp32/src/SDCard.cpp @@ -18,8 +18,10 @@ along with Grbl. If not, see . */ -#include "SDCard.h" -#include "MachineConfig.h" +#include "Config.h" +#ifdef ENABLE_SD_CARD +# include "SDCard.h" +# include "MachineConfig.h" File myFile; bool SD_ready_next = false; // Grbl has processed a line and is waiting for another @@ -32,7 +34,7 @@ static char comment[LINE_BUFFER_SIZE]; // Line to be executed. Z /*bool sd_mount() { if(!SD.begin()) { - report_status_message(Error::SdFailedMount, CLIENT_SERIAL); + report_status_message(Error::FsFailedMount, CLIENT_SERIAL); return false; } return true; @@ -42,11 +44,11 @@ void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) { //char temp_filename[128]; // to help filter by extension TODO: 128 needs a definition based on something File root = fs.open(dirname); if (!root) { - report_status_message(Error::SdFailedOpenDir, client); + report_status_message(Error::FsFailedOpenDir, client); return; } if (!root.isDirectory()) { - report_status_message(Error::SdDirNotFound, client); + report_status_message(Error::FsDirNotFound, client); return; } File file = root.openNextFile(); @@ -65,10 +67,10 @@ void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) { boolean openFile(fs::FS& fs, const char* path) { myFile = fs.open(path); if (!myFile) { - //report_status_message(Error::SdFailedRead, CLIENT_SERIAL); + //report_status_message(Error::FsFailedRead, CLIENT_SERIAL); return false; } - set_sd_state(SDCARD_BUSY_PRINTING); + set_sd_state(SDState::BusyPrinting); SD_ready_next = false; // this will get set to true when Grbl issues "ok" message sd_current_line_number = 0; return true; @@ -78,7 +80,7 @@ boolean closeFile() { if (!myFile) { return false; } - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); SD_ready_next = false; sd_current_line_number = 0; myFile.close(); @@ -95,7 +97,7 @@ boolean closeFile() { */ boolean readFileLine(char* line, int maxlen) { if (!myFile) { - report_status_message(Error::SdFailedRead, SD_client); + report_status_message(Error::FsFailedRead, SD_client); return false; } sd_current_line_number += 1; @@ -126,9 +128,9 @@ uint32_t sd_get_current_line_number() { return sd_current_line_number; } -uint8_t sd_state = SDCARD_IDLE; +SDState sd_state = SDState::Idle; -uint8_t get_sd_state(bool refresh) { +SDState get_sd_state(bool refresh) { // Before we use the SD library, we *must* make sure SPI is properly initialized. Re-initialization // fortunately doesn't change any of the settings. auto spiConfig = MachineConfig::instance()->_spi; @@ -144,13 +146,13 @@ uint8_t get_sd_state(bool refresh) { //no need to go further if SD detect is not correct if (SDCardDetPin->get() != Pin::UNDEFINED) { if (!((SDCardDetPin->get().read() == SDCARD_DET_VAL) ? true : false)) { - sd_state = SDCARD_NOT_PRESENT; + sd_state = SDState::NotPresent; return sd_state; } } //if busy doing something return state - if (!((sd_state == SDCARD_NOT_PRESENT) || (sd_state == SDCARD_IDLE))) { + if (!((sd_state == SDState::NotPresent) || (sd_state == SDState::Idle))) { return sd_state; } if (!refresh) { @@ -159,7 +161,7 @@ uint8_t get_sd_state(bool refresh) { //SD is idle or not detected, let see if still the case SD.end(); - sd_state = SDCARD_NOT_PRESENT; + sd_state = SDState::NotPresent; //using default value for speed ? should be parameter //refresh content if card was removed if (SD.begin((GRBL_SPI_SS == -1) ? SS : GRBL_SPI_SS, SPI, GRBL_SPI_FREQ, "/sd", 2)) { @@ -173,8 +175,8 @@ uint8_t get_sd_state(bool refresh) { } } -uint8_t set_sd_state(uint8_t flag) { - sd_state = flag; +SDState set_sd_state(SDState state) { + sd_state = state; return sd_state; } @@ -185,3 +187,4 @@ void sd_get_current_filename(char* name) { name[0] = 0; } } +#endif //ENABLE_SD_CARD diff --git a/Grbl_Esp32/src/SDCard.h b/Grbl_Esp32/src/SDCard.h index 9d441bbc..081ec3c5 100644 --- a/Grbl_Esp32/src/SDCard.h +++ b/Grbl_Esp32/src/SDCard.h @@ -22,19 +22,22 @@ const int SDCARD_DET_VAL = 0; -const int SDCARD_IDLE = 0; -const int SDCARD_NOT_PRESENT = 1; -const int SDCARD_BUSY_PRINTING = 2; -const int SDCARD_BUSY_UPLOADING = 4; -const int SDCARD_BUSY_PARSING = 8; +enum class SDState : uint8_t { + Idle = 0, + NotPresent = 1, + Busy = 2, + BusyPrinting = 2, + BusyUploading = 3, + BusyParsing = 4, +}; extern bool SD_ready_next; // Grbl has processed a line and is waiting for another extern uint8_t SD_client; extern WebUI::AuthenticationLevel SD_auth_level; //bool sd_mount(); -uint8_t get_sd_state(bool refresh); -uint8_t set_sd_state(uint8_t flag); +SDState get_sd_state(bool refresh); +SDState set_sd_state(SDState state); void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client); boolean openFile(fs::FS& fs, const char* path); boolean closeFile(); diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index caa78bb6..19425851 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -59,13 +59,13 @@ portMUX_TYPE myMutex = portMUX_INITIALIZER_UNLOCKED; -static TaskHandle_t serialCheckTaskHandle = 0; +static TaskHandle_t clientCheckTaskHandle = 0; WebUI::InputBuffer client_buffer[CLIENT_COUNT]; // create a buffer for each client // Returns the number of bytes available in a client buffer. -uint8_t serial_get_rx_buffer_available(uint8_t client) { - return client_buffer[client].availableforwrite(); +uint8_t client_get_rx_buffer_available(uint8_t client) { + return 128 - Uart0.available(); } void heapCheckTask(void* pvParameters) { @@ -79,106 +79,120 @@ void heapCheckTask(void* pvParameters) { vTaskDelay(3000 / portTICK_RATE_MS); // Yield to other tasks static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +#endif } } -void serial_init() { +void client_init() { #ifdef DEBUG_REPORT_HEAP_SIZE // For a 2000-word stack, uxTaskGetStackHighWaterMark reports 288 words available xTaskCreatePinnedToCore(heapCheckTask, "heapTask", 2000, NULL, 1, NULL, 1); #endif - Serial.begin(BAUD_RATE); - Serial.setRxBufferSize(256); - // reset all buffers - serial_reset_read_buffer(CLIENT_ALL); - grbl_send(CLIENT_SERIAL, "\r\n"); // create some white space after ESP32 boot info - serialCheckTaskHandle = 0; + Uart0.setPins(1, 3); // Tx 1, Rx 3 - standard hardware pins + Uart0.begin(BAUD_RATE, Uart::Data::Bits8, Uart::Stop::Bits1, Uart::Parity::None); + + client_reset_read_buffer(CLIENT_ALL); + Uart0.write("\r\n"); // create some white space after ESP32 boot info + clientCheckTaskHandle = 0; + // create a task to check for incoming data // For a 4096-word stack, uxTaskGetStackHighWaterMark reports 244 words available // after WebUI attaches. - xTaskCreatePinnedToCore(serialCheckTask, // task - "serialCheckTask", // name for task + xTaskCreatePinnedToCore(clientCheckTask, // task + "clientCheckTask", // name for task 4096, // size of task stack NULL, // parameters 1, // priority - &serialCheckTaskHandle, - 1 // core + &clientCheckTaskHandle, + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core ); } +static uint8_t getClientChar(uint8_t* data) { + int res; + if (client_buffer[CLIENT_SERIAL].availableforwrite() && (res = Uart0.read()) != -1) { + *data = res; + return CLIENT_SERIAL; + } + if (WebUI::inputBuffer.available()) { + *data = WebUI::inputBuffer.read(); + return CLIENT_INPUT; + } + //currently is wifi or BT but better to prepare both can be live +#ifdef ENABLE_BLUETOOTH + if (WebUI::SerialBT.hasClient()) { + if ((res = WebUI::SerialBT.read()) != -1) { + *data = res; + return CLIENT_BT; + } + } +#endif +#if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_IN) + if (WebUI::Serial2Socket.available()) { + *data = WebUI::Serial2Socket.read(); + return CLIENT_WEBUI; + } +#endif +#if defined(ENABLE_WIFI) && defined(ENABLE_TELNET) + if (WebUI::telnet_server.available()) { + *data = WebUI::telnet_server.read(); + return CLIENT_TELNET; + } +#endif + return CLIENT_ALL; +} + // this task runs and checks for data on all interfaces // REaltime stuff is acted upon, then characters are added to the appropriate buffer -void serialCheckTask(void* pvParameters) { - uint8_t data = 0; - uint8_t client = CLIENT_ALL; // who sent the data +void clientCheckTask(void* pvParameters) { + uint8_t data = 0; + uint8_t client; // who sent the data static UBaseType_t uxHighWaterMark = 0; while (true) { // run continuously - while (any_client_has_data()) { - if (Serial.available()) { - client = CLIENT_SERIAL; - data = Serial.read(); - } else if (WebUI::inputBuffer.available()) { - client = CLIENT_INPUT; - data = WebUI::inputBuffer.read(); - } else { - //currently is wifi or BT but better to prepare both can be live -#ifdef ENABLE_BLUETOOTH - if (WebUI::SerialBT.hasClient() && WebUI::SerialBT.available()) { - client = CLIENT_BT; - data = WebUI::SerialBT.read(); - - // Serial.write(data); // echo all data to serial. - } else { -#endif -#if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_IN) - if (WebUI::Serial2Socket.available()) { - client = CLIENT_WEBUI; - data = WebUI::Serial2Socket.read(); - } else { -#endif -#if defined(ENABLE_WIFI) && defined(ENABLE_TELNET) - if (WebUI::telnet_server.available()) { - client = CLIENT_TELNET; - data = WebUI::telnet_server.read(); - } -#endif -#if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_IN) - } -#endif -#ifdef ENABLE_BLUETOOTH - } -#endif - } - // Pick off realtime command characters directly from the serial stream. These characters are - // not passed into the main buffer, but these set system state flag bits for realtime execution. - if (is_realtime_command(data)) { - execute_realtime_command(static_cast(data), client); - } else { + // Pick off realtime command characters directly from the serial stream. These characters are + // not passed into the main buffer, but these set system state flag bits for realtime execution. + if (is_realtime_command(data)) { + execute_realtime_command(static_cast(data), client); + } else { +#if defined(ENABLE_SD_CARD) + if (get_sd_state(false) < SDState::Busy) { +#endif //ENABLE_SD_CARD vTaskEnterCritical(&myMutex); client_buffer[client].write(data); vTaskExitCritical(&myMutex); +#if defined(ENABLE_SD_CARD) + } else { + if (data == '\r' || data == '\n') { + grbl_sendf(client, "error %d\r\n", Error::AnotherInterfaceBusy); + grbl_msg_sendf(client, MsgLevel::Info, "SD card job running"); + } } - } // if something available - WebUI::COMMANDS::handle(); +#endif //ENABLE_SD_CARD + } + } // if something available + WebUI::COMMANDS::handle(); #ifdef ENABLE_WIFI - WebUI::wifi_config.handle(); + WebUI::wifi_config.handle(); #endif #ifdef ENABLE_BLUETOOTH - WebUI::bt_config.handle(); + WebUI::bt_config.handle(); #endif #if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_IN) - WebUI::Serial2Socket.handle_flush(); + WebUI::Serial2Socket.handle_flush(); #endif - vTaskDelay(1 / portTICK_RATE_MS); // Yield to other tasks + vTaskDelay(1 / portTICK_RATE_MS); // Yield to other tasks - static UBaseType_t uxHighWaterMark = 0; - reportTaskStackSize(uxHighWaterMark); - } // while(true) + static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK + reportTaskStackSize(uxHighWaterMark); +#endif +} } -void serial_reset_read_buffer(uint8_t client) { +void client_reset_read_buffer(uint8_t client) { for (uint8_t client_num = 0; client_num < CLIENT_COUNT; client_num++) { if (client == client_num || client == CLIENT_ALL) { client_buffer[client_num].begin(); @@ -186,38 +200,12 @@ void serial_reset_read_buffer(uint8_t client) { } } -// Writes one byte to the TX serial buffer. Called by main program. -void serial_write(uint8_t data) { - Serial.write((char)data); -} - -// Fetches the first byte in the serial read buffer. Called by protocol loop. -uint8_t serial_read(uint8_t client) { - uint8_t data; +// Fetches the first byte in the client read buffer. Called by protocol loop. +int client_read(uint8_t client) { vTaskEnterCritical(&myMutex); - if (client_buffer[client].available()) { - data = client_buffer[client].read(); - vTaskExitCritical(&myMutex); - //Serial.write((char)data); - return data; - } else { - vTaskExitCritical(&myMutex); - return SERIAL_NO_DATA; - } -} - -bool any_client_has_data() { - return (Serial.available() || WebUI::inputBuffer.available() -#ifdef ENABLE_BLUETOOTH - || (WebUI::SerialBT.hasClient() && WebUI::SerialBT.available()) -#endif -#if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_IN) - || WebUI::Serial2Socket.available() -#endif -#if defined(ENABLE_WIFI) && defined(ENABLE_TELNET) - || WebUI::telnet_server.available() -#endif - ); + int data = client_buffer[client].read(); + vTaskExitCritical(&myMutex); + return data; } // checks to see if a character is a realtime character @@ -233,6 +221,7 @@ bool is_realtime_command(uint8_t data) { void execute_realtime_command(Cmd command, uint8_t client) { switch (command) { case Cmd::Reset: + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Debug, "Cmd::Reset"); mc_reset(); // Call motion control reset routine. break; case Cmd::StatusReport: @@ -334,3 +323,32 @@ void execute_realtime_command(Cmd command, uint8_t client) { break; } } + +void client_write(uint8_t client, const char* text) { + if (client == CLIENT_INPUT) { + return; + } +#ifdef ENABLE_BLUETOOTH + if (WebUI::SerialBT.hasClient() && (client == CLIENT_BT || client == CLIENT_ALL)) { + WebUI::SerialBT.print(text); + //delay(10); // possible fix for dropped characters + } +#endif +#if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_OUT) + if (client == CLIENT_WEBUI || client == CLIENT_ALL) { + WebUI::Serial2Socket.write((const uint8_t*)text, strlen(text)); + } +#endif +#if defined(ENABLE_WIFI) && defined(ENABLE_TELNET) + if (client == CLIENT_TELNET || client == CLIENT_ALL) { + WebUI::telnet_server.write((const uint8_t*)text, strlen(text)); + } +#endif + if (client == CLIENT_SERIAL || client == CLIENT_ALL) { +#ifdef REVERT_TO_ARDUINO_SERIAL + Serial.write(text); +#else + Uart0.write(text); +#endif + } +} diff --git a/Grbl_Esp32/src/Serial.h b/Grbl_Esp32/src/Serial.h index 7cca0d94..ec290050 100644 --- a/Grbl_Esp32/src/Serial.h +++ b/Grbl_Esp32/src/Serial.h @@ -20,7 +20,7 @@ along with Grbl. If not, see . */ -#include "Grbl.h" +#include "stdint.h" #ifndef RX_BUFFER_SIZE # define RX_BUFFER_SIZE 256 @@ -33,24 +33,22 @@ # endif #endif -const float SERIAL_NO_DATA = 0xff; - // a task to read for incoming data from serial port -void serialCheckTask(void* pvParameters); +void clientCheckTask(void* pvParameters); + +void client_write(uint8_t client, const char* text); -void serial_write(uint8_t data); // Fetches the first byte in the serial read buffer. Called by main program. -uint8_t serial_read(uint8_t client); +int client_read(uint8_t client); // See if the character is an action command like feedhold or jogging. If so, do the action and return true uint8_t check_action_command(uint8_t data); -void serial_init(); -void serial_reset_read_buffer(uint8_t client); +void client_init(); +void client_reset_read_buffer(uint8_t client); // Returns the number of bytes available in the RX serial buffer. -uint8_t serial_get_rx_buffer_available(uint8_t client); +uint8_t client_get_rx_buffer_available(uint8_t client); void execute_realtime_command(Cmd command, uint8_t client); -bool any_client_has_data(); bool is_realtime_command(uint8_t data); diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index c0af4462..cffe76ca 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -3,13 +3,28 @@ #include #include +bool anyState() { + return false; +} +bool idleOrJog() { + return sys.state != State::Idle && sys.state != State::Jog; +} +bool idleOrAlarm() { + return sys.state != State::Idle && sys.state != State::Alarm; +} +bool notCycleOrHold() { + return sys.state == State::Cycle && sys.state == State::Hold; +} + Word::Word(type_t type, permissions_t permissions, const char* description, const char* grblName, const char* fullName) : _description(description), _grblName(grblName), _fullName(fullName), _type(type), _permissions(permissions) {} Command* Command::List = NULL; -Command::Command(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName) : - Word(type, permissions, description, grblName, fullName) { +Command::Command( + const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName, bool (*cmdChecker)()) : + Word(type, permissions, description, grblName, fullName), + _cmdChecker(cmdChecker) { link = List; List = this; } @@ -126,6 +141,7 @@ Error IntSetting::setStringValue(char* s) { _storedValue = convertedValue; } } + check(NULL); return Error::Ok; } @@ -223,6 +239,7 @@ Error AxisMaskSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -323,6 +340,7 @@ Error FloatSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -412,6 +430,7 @@ Error StringSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -519,11 +538,15 @@ void PinSetting::addWebui(WebUI::JSONencoder* j) { typedef std::map enum_opt_t; -EnumSetting::EnumSetting( - const char* description, type_t type, permissions_t permissions, const char* grblName, const char* name, int8_t defVal, enum_opt_t* opts) - // No checker function because enumerations have an exact set of value - : - Setting(description, type, permissions, grblName, name, NULL), +EnumSetting::EnumSetting(const char* description, + type_t type, + permissions_t permissions, + const char* grblName, + const char* name, + int8_t defVal, + enum_opt_t* opts, + bool (*checker)(char*) = NULL) : + Setting(description, type, permissions, grblName, name, checker), _defaultValue(defVal), _options(opts) {} void EnumSetting::load() { @@ -548,7 +571,11 @@ void EnumSetting::setDefault() { // This is necessary for WebUI, which uses the number // for setting. Error EnumSetting::setStringValue(char* s) { - s = trim(s); + s = trim(s); + Error err = check(s); + if (err != Error::Ok) { + return err; + } enum_opt_t::iterator it = _options->find(s); if (it == _options->end()) { // If we don't find the value in keys, look for it in the numeric values @@ -574,7 +601,7 @@ Error EnumSetting::setStringValue(char* s) { } _currentValue = it->second; if (_storedValue != _currentValue) { - if (_storedValue == _defaultValue) { + if (_currentValue == _defaultValue) { nvs_erase_key(_handle, _keyName); } else { if (nvs_set_i8(_handle, _keyName, _currentValue)) { @@ -583,6 +610,7 @@ Error EnumSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -662,6 +690,7 @@ Error FlagSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } const char* FlagSetting::getDefaultString() { @@ -742,6 +771,7 @@ Error IPaddrSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -766,7 +796,7 @@ void IPaddrSetting::addWebui(WebUI::JSONencoder* j) { AxisSettings::AxisSettings(const char* axisName) : name(axisName) {} Error GrblCommand::action(char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { - if (_checker && _checker()) { + if (_cmdChecker && _cmdChecker()) { return Error::IdleError; } return _action((const char*)value, auth_level, out); diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index 9aafeec8..ae4ca1cb 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -78,12 +78,14 @@ public: class Command : public Word { protected: Command* link; // linked list of setting objects + bool (*_cmdChecker)(); + public: static Command* List; Command* next() { return link; } ~Command() {} - Command(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName); + Command(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName, bool (*cmdChecker)()); // The default implementation of addWebui() does nothing. // Derived classes may override it to do something. @@ -368,10 +370,17 @@ public: const char* grblName, const char* name, int8_t defVal, - enum_opt_t* opts); + enum_opt_t* opts, + bool (*checker)(char*)); - EnumSetting(type_t type, permissions_t permissions, const char* grblName, const char* name, int8_t defVal, enum_opt_t* opts) : - EnumSetting(NULL, type, permissions, grblName, name, defVal, opts) {} + EnumSetting(type_t type, + permissions_t permissions, + const char* grblName, + const char* name, + int8_t defVal, + enum_opt_t* opts, + bool (*checker)(char*) = NULL) : + EnumSetting(NULL, type, permissions, grblName, name, defVal, opts, checker) {} void load(); void setDefault(); @@ -460,6 +469,12 @@ public: AxisSettings(const char* axisName); }; + +extern bool idleOrJog(); +extern bool idleOrAlarm(); +extern bool anyState(); +extern bool notCycleOrHold(); + class WebCommand : public Command { private: Error (*_action)(char*, WebUI::AuthenticationLevel); @@ -471,31 +486,40 @@ public: permissions_t permissions, const char* grblName, const char* name, - Error (*action)(char*, WebUI::AuthenticationLevel)) : - Command(description, type, permissions, grblName, name), + Error (*action)(char*, WebUI::AuthenticationLevel), + bool (*cmdChecker)()) : + Command(description, type, permissions, grblName, name, cmdChecker), _action(action) {} + + WebCommand(const char* description, + type_t type, + permissions_t permissions, + const char* grblName, + const char* name, + Error (*action)(char*, WebUI::AuthenticationLevel)) : + WebCommand(description, type, permissions, grblName, name, action, idleOrAlarm) {} + Error action(char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* response); }; class GrblCommand : public Command { private: Error (*_action)(const char*, WebUI::AuthenticationLevel, WebUI::ESPResponseStream*); - bool (*_checker)(); public: GrblCommand(const char* grblName, const char* name, Error (*action)(const char*, WebUI::AuthenticationLevel, WebUI::ESPResponseStream*), - bool (*checker)(), + bool (*cmdChecker)(), permissions_t auth) : - Command(NULL, GRBLCMD, auth, grblName, name), - _action(action), _checker(checker) {} + Command(NULL, GRBLCMD, auth, grblName, name, cmdChecker), + _action(action) {} GrblCommand(const char* grblName, const char* name, Error (*action)(const char*, WebUI::AuthenticationLevel, WebUI::ESPResponseStream*), - bool (*checker)(void)) : - GrblCommand(grblName, name, action, checker, WG) {} + bool (*cmdChecker)()) : + GrblCommand(grblName, name, action, cmdChecker, WG) {} Error action(char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* response); }; diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 6a0aed6f..54f45679 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -1,7 +1,5 @@ #include "Grbl.h" -bool motorSettingChanged = false; - FlagSetting* verbose_errors; FakeSetting* number_axis; @@ -12,6 +10,8 @@ StringSetting* build_info; IntSetting* pulse_microseconds; IntSetting* stepper_idle_lock_time; +IntSetting* direction_delay_microseconds; +IntSetting* enable_delay_microseconds; AxisMaskSetting* step_invert_mask; AxisMaskSetting* dir_invert_mask; @@ -32,6 +32,7 @@ FlagSetting* homing_enable; // TODO Settings - also need to clear, but not set, soft_limits FlagSetting* laser_mode; // TODO Settings - also need to call my_spindle->init; +IntSetting* laser_full_power; IntSetting* status_mask; FloatSetting* junction_deviation; @@ -47,6 +48,7 @@ FloatSetting* rpm_max; FloatSetting* rpm_min; FloatSetting* spindle_delay_spinup; FloatSetting* spindle_delay_spindown; +FloatSetting* coolant_start_delay; FlagSetting* spindle_enbl_off_with_zero_speed; FlagSetting* spindle_enable_invert; FlagSetting* spindle_output_invert; @@ -58,6 +60,8 @@ IntSetting* spindle_pwm_bit_precision; EnumSetting* spindle_type; +EnumSetting* message_level; + enum_opt_t spindleTypes = { // clang-format off { "NONE", int8_t(SpindleType::NONE) }, @@ -72,6 +76,17 @@ enum_opt_t spindleTypes = { // clang-format on }; +enum_opt_t messageLevels = { + // clang-format off + { "None", int8_t(MsgLevel::None) }, + { "Error", int8_t(MsgLevel::Error) }, + { "Warning", int8_t(MsgLevel::Warning) }, + { "Info", int8_t(MsgLevel::Info) }, + { "Debug", int8_t(MsgLevel::Debug) }, + { "Verbose", int8_t(MsgLevel::Verbose) }, + // clang-format on +}; + AxisSettings* x_axis_settings; AxisSettings* y_axis_settings; AxisSettings* z_axis_settings; @@ -81,6 +96,11 @@ AxisSettings* c_axis_settings; AxisSettings* axis_settings[MAX_N_AXIS]; +StringSetting* user_macro0; +StringSetting* user_macro1; +StringSetting* user_macro2; +StringSetting* user_macro3; + typedef struct { const char* name; float steps_per_mm; @@ -166,34 +186,37 @@ static const char* makename(const char* axisName, const char* tail) { } static bool checkStartupLine(char* value) { + if (!value) { // No POST functionality + return true; + } if (sys.state != State::Idle) { return false; } return gc_execute_line(value, CLIENT_SERIAL) == Error::Ok; } -static bool checkStallguard(char* value) { - motorSettingChanged = true; +static bool postMotorSetting(char* value) { + if (!value) { + motors_read_settings(); + } return true; } -static bool checkMicrosteps(char* value) { - motorSettingChanged = true; - return true; -} - -static bool checkRunCurrent(char* value) { - motorSettingChanged = true; - return true; -} - -static bool checkHoldcurrent(char* value) { - motorSettingChanged = true; - return true; -} - -static bool checkStallguardDebugMask(char* val) { - motorSettingChanged = true; +static bool checkSpindleChange(char* val) { + if (!val) { + // if not in disable (M5) ... + if (gc_state.modal.spindle != SpindleState::Disable) { + gc_state.modal.spindle = SpindleState::Disable; + if (spindle->use_delays && spindle_delay_spindown->get() != 0) { // old spindle + vTaskDelay(spindle_delay_spindown->get() * 1000); + } + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle turned off with setting change"); + } + gc_state.spindle_speed = 0; // Set S value to 0 + spindle->deinit(); // old spindle + Spindles::Spindle::select(); // get new spindle + return true; + } return true; } @@ -212,17 +235,7 @@ void make_coordinate(CoordIndex index, const char* name) { auto coord = new Coordinates(name); coords[index] = coord; if (!coord->load()) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Propagating %s data to NVS format", coord->getName()); - // If coord->load() returns false it means that no entry - // was present in non-volatile storage. In that case we - // first look for an old-format entry in the EEPROM section. - // If an entry is present some number of float values at - // the beginning of coord_data will be overwritten with - // the EEPROM data, and the rest will remain at 0.0. - // If no old-format entry is present, all will remain 0.0 - // Regardless, we create a new entry with that data. - (void)old_settings_read_coord_data(index, coord_data); - coords[index]->set(coord_data); + coords[index]->setDefault(); } } @@ -264,34 +277,34 @@ void make_settings() { for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new IntSetting( - EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 63, checkStallguard); + EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 255, postMotorSetting); setting->setAxis(axis); axis_settings[axis]->stallguard = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new IntSetting( - EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, checkMicrosteps); + EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, postMotorSetting); setting->setAxis(axis); axis_settings[axis]->microsteps = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new FloatSetting( - EXTENDED, WG, makeGrblName(axis, 150), makename(def->name, "Current/Hold"), def->hold_current, 0.05, 20.0, checkHoldcurrent); // Amps + EXTENDED, WG, makeGrblName(axis, 150), makename(def->name, "Current/Hold"), def->hold_current, 0.05, 20.0, postMotorSetting); // Amps setting->setAxis(axis); axis_settings[axis]->hold_current = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new FloatSetting( - EXTENDED, WG, makeGrblName(axis, 140), makename(def->name, "Current/Run"), def->run_current, 0.0, 20.0, checkRunCurrent); // Amps + EXTENDED, WG, makeGrblName(axis, 140), makename(def->name, "Current/Run"), def->run_current, 0.0, 20.0, postMotorSetting); // Amps setting->setAxis(axis); axis_settings[axis]->run_current = setting; } for (axis = MAX_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); + auto setting = new FloatSetting(GRBL, WG, makeGrblName(axis, 130), makename(def->name, "MaxTravel"), def->max_travel, 0, 100000.0); setting->setAxis(axis); axis_settings[axis]->max_travel = setting; } @@ -325,31 +338,41 @@ void make_settings() { } // Spindle Settings - spindle_pwm_max_value = new FloatSetting(EXTENDED, WG, "36", "Spindle/PWM/Max", DEFAULT_SPINDLE_MAX_VALUE, 0.0, 100.0); - spindle_pwm_min_value = new FloatSetting(EXTENDED, WG, "35", "Spindle/PWM/Min", DEFAULT_SPINDLE_MIN_VALUE, 0.0, 100.0); - spindle_pwm_off_value = - new FloatSetting(EXTENDED, WG, "34", "Spindle/PWM/Off", DEFAULT_SPINDLE_OFF_VALUE, 0.0, 100.0); // these are percentages - // IntSetting spindle_pwm_bit_precision(EXTENDED, WG, "Spindle/PWM/Precision", DEFAULT_SPINDLE_BIT_PRECISION, 1, 16); - spindle_pwm_freq = new FloatSetting(EXTENDED, WG, "33", "Spindle/PWM/Frequency", DEFAULT_SPINDLE_FREQ, 0, 100000); - spindle_output_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/PWM/Invert", DEFAULT_INVERT_SPINDLE_OUTPUT_PIN); + spindle_type = + new EnumSetting(NULL, EXTENDED, WG, NULL, "Spindle/Type", static_cast(SPINDLE_TYPE), &spindleTypes, checkSpindleChange); - spindle_delay_spinup = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30); - spindle_delay_spindown = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30); + spindle_pwm_max_value = + new FloatSetting(EXTENDED, WG, "36", "Spindle/PWM/Max", DEFAULT_SPINDLE_MAX_VALUE, 0.0, 100.0, checkSpindleChange); + spindle_pwm_min_value = + new FloatSetting(EXTENDED, WG, "35", "Spindle/PWM/Min", DEFAULT_SPINDLE_MIN_VALUE, 0.0, 100.0, checkSpindleChange); + spindle_pwm_off_value = new FloatSetting( + EXTENDED, WG, "34", "Spindle/PWM/Off", DEFAULT_SPINDLE_OFF_VALUE, 0.0, 100.0, checkSpindleChange); // these are percentages + // IntSetting spindle_pwm_bit_precision(EXTENDED, WG, "Spindle/PWM/Precision", DEFAULT_SPINDLE_BIT_PRECISION, 1, 16); + spindle_pwm_freq = new FloatSetting(EXTENDED, WG, "33", "Spindle/PWM/Frequency", DEFAULT_SPINDLE_FREQ, 0, 100000, checkSpindleChange); + spindle_output_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/PWM/Invert", DEFAULT_INVERT_SPINDLE_OUTPUT_PIN, checkSpindleChange); + + spindle_delay_spinup = + new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange); + spindle_delay_spindown = + new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange); + coolant_start_delay = new FloatSetting(EXTENDED, WG, NULL, "Coolant/Delay/TurnOn", DEFAULT_COOLANT_DELAY_TURNON, 0, 30); spindle_enbl_off_with_zero_speed = - new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED); + new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED, checkSpindleChange); - spindle_enable_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/Invert", DEFAULT_INVERT_SPINDLE_ENABLE_PIN); + spindle_enable_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/Invert", DEFAULT_INVERT_SPINDLE_ENABLE_PIN, checkSpindleChange); // GRBL Non-numbered settings - startup_line_0 = new StringSetting(GRBL, WG, "N0", "GCode/Line0", "", checkStartupLine); - startup_line_1 = new StringSetting(GRBL, WG, "N1", "GCode/Line1", "", checkStartupLine); + startup_line_0 = new StringSetting(EXTENDED, WG, "N0", "GCode/Line0", "", checkStartupLine); + startup_line_1 = new StringSetting(EXTENDED, WG, "N1", "GCode/Line1", "", checkStartupLine); // GRBL Numbered Settings - laser_mode = new FlagSetting(GRBL, WG, "32", "GCode/LaserMode", DEFAULT_LASER_MODE); + laser_mode = new FlagSetting(GRBL, WG, "32", "GCode/LaserMode", DEFAULT_LASER_MODE); + laser_full_power = new IntSetting(EXTENDED, WG, NULL, "Laser/FullPower", DEFAULT_LASER_FULL_POWER, 0, 10000, checkSpindleChange); + // TODO Settings - also need to call my_spindle->init(); - rpm_min = new FloatSetting(GRBL, WG, "31", "GCode/MinS", DEFAULT_SPINDLE_RPM_MIN, 0, 100000); - rpm_max = new FloatSetting(GRBL, WG, "30", "GCode/MaxS", DEFAULT_SPINDLE_RPM_MAX, 0, 100000); + rpm_min = new FloatSetting(GRBL, WG, "31", "GCode/MinS", DEFAULT_SPINDLE_RPM_MIN, 0, 100000, checkSpindleChange); + rpm_max = new FloatSetting(GRBL, WG, "30", "GCode/MaxS", DEFAULT_SPINDLE_RPM_MAX, 0, 100000, checkSpindleChange); homing_pulloff = new FloatSetting(GRBL, WG, "27", "Homing/Pulloff", DEFAULT_HOMING_PULLOFF, 0, 1000); homing_debounce = new FloatSetting(GRBL, WG, "26", "Homing/Debounce", DEFAULT_HOMING_DEBOUNCE_DELAY, 0, 10000); @@ -373,22 +396,31 @@ void make_settings() { junction_deviation = new FloatSetting(GRBL, WG, "11", "GCode/JunctionDeviation", DEFAULT_JUNCTION_DEVIATION, 0, 10); status_mask = new IntSetting(GRBL, WG, "10", "Report/Status", DEFAULT_STATUS_REPORT_MASK, 0, 3); - probe_invert = new FlagSetting(GRBL, WG, "6", "Probe/Invert", DEFAULT_INVERT_PROBE_PIN); - limit_invert = new FlagSetting(GRBL, WG, "5", "Limits/Invert", DEFAULT_INVERT_LIMIT_PINS); - step_enable_invert = new FlagSetting(GRBL, WG, "4", "Stepper/EnableInvert", DEFAULT_INVERT_ST_ENABLE); - dir_invert_mask = new AxisMaskSetting(GRBL, WG, "3", "Stepper/DirInvert", DEFAULT_DIRECTION_INVERT_MASK); - step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK); - stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255); - pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); - spindle_type = new EnumSetting(NULL, EXTENDED, WG, NULL, "Spindle/Type", static_cast(SPINDLE_TYPE), &spindleTypes); - stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, checkStallguardDebugMask); + probe_invert = new FlagSetting(GRBL, WG, "6", "Probe/Invert", DEFAULT_INVERT_PROBE_PIN); + limit_invert = new FlagSetting(GRBL, WG, "5", "Limits/Invert", DEFAULT_INVERT_LIMIT_PINS); + step_enable_invert = new FlagSetting(GRBL, WG, "4", "Stepper/EnableInvert", DEFAULT_INVERT_ST_ENABLE); + dir_invert_mask = new AxisMaskSetting(GRBL, WG, "3", "Stepper/DirInvert", DEFAULT_DIRECTION_INVERT_MASK, postMotorSetting); + step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK, postMotorSetting); + stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255); + pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); + direction_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Direction/Delay", STEP_PULSE_DELAY, 0, 1000); + enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", DEFAULT_STEP_ENABLE_DELAY, 0, 1000); // microseconds + + stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, postMotorSetting); - homing_cycle[0] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle0", DEFAULT_HOMING_CYCLE_0); - homing_cycle[1] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle1", DEFAULT_HOMING_CYCLE_1); - homing_cycle[2] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle2", DEFAULT_HOMING_CYCLE_2); - homing_cycle[3] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle3", DEFAULT_HOMING_CYCLE_3); - homing_cycle[4] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle4", DEFAULT_HOMING_CYCLE_4); homing_cycle[5] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle5", DEFAULT_HOMING_CYCLE_5); + homing_cycle[4] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle4", DEFAULT_HOMING_CYCLE_4); + homing_cycle[3] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle3", DEFAULT_HOMING_CYCLE_3); + homing_cycle[2] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle2", DEFAULT_HOMING_CYCLE_2); + homing_cycle[1] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle1", DEFAULT_HOMING_CYCLE_1); + homing_cycle[0] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle0", DEFAULT_HOMING_CYCLE_0); + + user_macro3 = new StringSetting(EXTENDED, WG, NULL, "User/Macro3", DEFAULT_USER_MACRO3); + user_macro2 = new StringSetting(EXTENDED, WG, NULL, "User/Macro2", DEFAULT_USER_MACRO2); + user_macro1 = new StringSetting(EXTENDED, WG, NULL, "User/Macro1", DEFAULT_USER_MACRO1); + user_macro0 = new StringSetting(EXTENDED, WG, NULL, "User/Macro0", DEFAULT_USER_MACRO0); + + message_level = +new EnumSetting(NULL, EXTENDED, WG, NULL, "Message/Level", static_cast(MsgLevel::Info), &messageLevels, NULL); make_pin_settings(); // Created in PinSettingsDefinitions.cpp } diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index 01a378ca..2e772f7b 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -1,7 +1,5 @@ #pragma once -extern bool motorSettingChanged; - extern FlagSetting* verbose_errors; extern FakeSetting* number_axis; @@ -20,6 +18,8 @@ extern StringSetting* build_info; extern IntSetting* pulse_microseconds; extern IntSetting* stepper_idle_lock_time; +extern IntSetting* direction_delay_microseconds; +extern IntSetting* enable_delay_microseconds; extern AxisMaskSetting* step_invert_mask; extern AxisMaskSetting* dir_invert_mask; @@ -35,6 +35,7 @@ extern FlagSetting* soft_limits; extern FlagSetting* hard_limits; extern FlagSetting* homing_enable; extern FlagSetting* laser_mode; +extern IntSetting* laser_full_power; extern IntSetting* status_mask; extern FloatSetting* junction_deviation; @@ -49,6 +50,7 @@ extern FloatSetting* rpm_max; extern FloatSetting* rpm_min; extern FloatSetting* spindle_delay_spinup; extern FloatSetting* spindle_delay_spindown; +extern FloatSetting* coolant_start_delay; extern FlagSetting* spindle_enbl_off_with_zero_speed; extern FlagSetting* spindle_enable_invert; extern FlagSetting* spindle_output_invert; @@ -60,6 +62,13 @@ extern IntSetting* spindle_pwm_bit_precision; extern EnumSetting* spindle_type; +extern StringSetting* user_macro0; +extern StringSetting* user_macro1; +extern StringSetting* user_macro2; +extern StringSetting* user_macro3; + +extern EnumSetting* message_level; + extern AxisMaskSetting* stallguard_debug_mask; // Pins: diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.cpp b/Grbl_Esp32/src/Spindles/10vSpindle.cpp index bc23ae60..0f1b5124 100644 --- a/Grbl_Esp32/src/Spindles/10vSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/10vSpindle.cpp @@ -35,7 +35,7 @@ namespace Spindles { _reverse_pin = SpindleReversePin->get(); if (_output_pin == Pin::UNDEFINED) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin not defined"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle output pin not defined"); return; // We cannot continue without the output pin } @@ -59,7 +59,7 @@ namespace Spindles { // prints the startup message of the spindle config void _10v::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "0-10V spindle Out:%s Enbl:%s, Dir:%s, Fwd:%s, Rev:%s, Freq:%dHz Res:%dbits", _output_pin.name().c_str(), @@ -137,7 +137,6 @@ namespace Spindles { } void _10v::set_enable_pin(bool enable) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle::_10v::set_enable_pin"); if (_off_with_zero_speed && sys.spindle_speed == 0) { enable = false; } @@ -157,9 +156,17 @@ namespace Spindles { } void _10v::set_dir_pin(bool Clockwise) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle::_10v::set_dir_pin"); _direction_pin.write(Clockwise); _forward_pin.write(Clockwise); _reverse_pin.write(!Clockwise); } + + void _10v::deinit() { + _enable_pin.setAttr(Pin::Attr::Input); + _direction_pin.setAttr(Pin::Attr::Input); + _forward_pin.setAttr(Pin::Attr::Input); + _reverse_pin.setAttr(Pin::Attr::Input); + ledcDetachPin(_output_pin.getNative(Pin::Capabilities::PWM)); + _output_pin.setAttr(Pin::Attr::Input); + } } diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.h b/Grbl_Esp32/src/Spindles/10vSpindle.h index e56f4cc4..70020e31 100644 --- a/Grbl_Esp32/src/Spindles/10vSpindle.h +++ b/Grbl_Esp32/src/Spindles/10vSpindle.h @@ -45,6 +45,7 @@ namespace Spindles { SpindleState get_state() override; void stop() override; + void deinit() override; virtual ~_10v() {} diff --git a/Grbl_Esp32/src/Spindles/BESCSpindle.cpp b/Grbl_Esp32/src/Spindles/BESCSpindle.cpp index 25502951..357e0d6c 100644 --- a/Grbl_Esp32/src/Spindles/BESCSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/BESCSpindle.cpp @@ -55,7 +55,7 @@ namespace Spindles { get_pins_and_settings(); // these gets the standard PWM settings, but many need to be changed for BESC if (_output_pin == Pin::UNDEFINED) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: BESC output pin not defined"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: BESC output pin not defined"); return; // We cannot continue without the output pin } @@ -84,7 +84,7 @@ namespace Spindles { // prints the startup message of the spindle config void BESC::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "BESC spindle on Pin:%s Min:%0.2fms Max:%0.2fms Freq:%dHz Res:%dbits", _output_pin.name().c_str(), diff --git a/Grbl_Esp32/src/Spindles/DacSpindle.cpp b/Grbl_Esp32/src/Spindles/DacSpindle.cpp index 27399d81..8f4d3077 100644 --- a/Grbl_Esp32/src/Spindles/DacSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/DacSpindle.cpp @@ -40,7 +40,7 @@ namespace Spindles { if (!_output_pin.capabilities().has(Pin::Capabilities::DAC)) { // DAC can only be used on these pins _gpio_ok = false; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "DAC spindle pin invalid %s (pin 25 or 26 only)", _output_pin.name().c_str()); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "DAC spindle pin invalid %s (pin 25 or 26 only)", _output_pin.name().c_str()); return; } @@ -54,7 +54,7 @@ namespace Spindles { } void Dac::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "DAC spindle Output:%s, Enbl:%s, Dir:%s, Res:8bits", _output_pin.name().c_str(), @@ -85,7 +85,7 @@ namespace Spindles { rpm = _min_rpm; sys.spindle_speed = rpm; pwm_value = 0; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RPM less than min RPM:%5.2f %d", rpm, pwm_value); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RPM less than min RPM:%5.2f %d", rpm, pwm_value); } } else { // Compute intermediate PWM value with linear spindle speed model. diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp index b18249e5..70c0db8f 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp @@ -24,21 +24,14 @@ Remove power before changing bits. The documentation is okay once you get how it works, but unfortunately - incomplete... See H2ASpindle.md for the remainder of the docs that I + incomplete... See H2ASpindle.md for the remainder of the docs that I managed to piece together. */ -#include - namespace Spindles { - void H2A::default_modbus_settings(uart_config_t& uart) { - // sets the uart to 19200 8E1 - VFD::default_modbus_settings(uart); - - uart.baud_rate = 19200; - uart.data_bits = UART_DATA_8_BITS; - uart.parity = UART_PARITY_EVEN; - uart.stop_bits = UART_STOP_BITS_1; + H2A::H2A() : VFD() { + _baudrate = 19200; + _parity = Uart::Parity::Even; } void H2A::direction_command(SpindleState mode, ModbusCommand& data) { @@ -80,31 +73,35 @@ namespace Spindles { data.msg[5] = uint8_t(speed & 0xFF); } - H2A::response_parser H2A::get_max_rpm(ModbusCommand& data) { - // NOTE: data length is excluding the CRC16 checksum. - data.tx_length = 6; - data.rx_length = 8; + VFD::response_parser H2A::initialization_sequence(int index, ModbusCommand& data) { + if (index == -1) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 8; - // Send: 01 03 B005 0002 - data.msg[1] = 0x03; // READ - data.msg[2] = 0xB0; // B0.05 = Get RPM - data.msg[3] = 0x05; - data.msg[4] = 0x00; // Read 2 values - data.msg[5] = 0x02; + // Send: 01 03 B005 0002 + data.msg[1] = 0x03; // READ + data.msg[2] = 0xB0; // B0.05 = Get RPM + data.msg[3] = 0x05; + data.msg[4] = 0x00; // Read 2 values + data.msg[5] = 0x02; - // Recv: 01 03 00 04 5D C0 03 F6 - // -- -- = 24000 (val #1) - return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { - uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]); - vfd->_max_rpm = rpm; + // Recv: 01 03 00 04 5D C0 03 F6 + // -- -- = 24000 (val #1) + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]); + vfd->_max_rpm = rpm; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); - return true; - }; + return true; + }; + } else { + return nullptr; + } } - H2A::response_parser H2A::get_current_rpm(ModbusCommand& data) { + VFD::response_parser H2A::get_current_rpm(ModbusCommand& data) { // NOTE: data length is excluding the CRC16 checksum. data.tx_length = 6; data.rx_length = 8; @@ -118,16 +115,16 @@ namespace Spindles { // Recv: 01 03 0004 095D 0000 // ---- = 2397 (val #1) - - // TODO: What are we going to do with this? Update sys.spindle_speed? Update vfd state? return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]); + // Set current RPM value? Somewhere? + vfd->_sync_rpm = rpm; return true; }; } - H2A::response_parser H2A::get_current_direction(ModbusCommand& data) { + VFD::response_parser H2A::get_current_direction(ModbusCommand& data) { // NOTE: data length is excluding the CRC16 checksum. data.tx_length = 6; data.rx_length = 6; diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.h b/Grbl_Esp32/src/Spindles/H2ASpindle.h index d206603b..59dac1dc 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.h +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.h @@ -24,14 +24,18 @@ namespace Spindles { class H2A : public VFD { protected: - void default_modbus_settings(uart_config_t& uart) override; - void direction_command(SpindleState mode, ModbusCommand& data) override; void set_speed_command(uint32_t rpm, ModbusCommand& data) override; - response_parser get_max_rpm(ModbusCommand& data) override; + response_parser initialization_sequence(int index, ModbusCommand& data) override; response_parser get_current_rpm(ModbusCommand& data) override; response_parser get_current_direction(ModbusCommand& data) override; response_parser get_status_ok(ModbusCommand& data) override { return nullptr; } + + bool supports_actual_rpm() const override { return true; } + bool safety_polling() const override { return false; } + + public: + H2A(); }; } diff --git a/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp b/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp index e0172de3..cf2e6b80 100644 --- a/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp @@ -41,24 +41,31 @@ Protocol Details - VFD frequencies are in Hz. Multiply by 60 for RPM + A lot of good information about the details of all these parameters and how they should + be setup can be found on this page: + https://community.carbide3d.com/t/vfd-parameters-huanyang-model/15459/7 . - before using spindle, VFD must be setup for RS485 and match your spindle - PD001 2 RS485 Control of run commands - PD002 2 RS485 Control of operating frequency - PD005 400 Maximum frequency Hz (Typical for spindles) - PD011 120 Min Speed (Recommend Aircooled=120 Water=100) - PD014 10 Acceleration time (Test to optimize) - PD015 10 Deceleration time (Test to optimize) - PD023 1 Reverse run enabled - PD142 3.7 Max current Amps (0.8kw=3.7 1.5kw=7.0, 2.2kw=??) - PD143 2 Poles most are 2 (I think this is only used for RPM calc from Hz) - PD163 1 RS485 Address: 1 (Typical. OK to change...see below) - PD164 1 RS485 Baud rate: 9600 (Typical. OK to change...see below) - PD165 3 RS485 Mode: RTU, 8N1 + Before using spindle, VFD must be setup for RS485 and match your spindle: + + PD004 400 Base frequency as rated on my spindle (default was 50) + PD005 400 Maximum frequency Hz (Typical for spindles) + PD011 120 Min Speed (Recommend Aircooled=120 Water=100) + PD014 10 Acceleration time (Test to optimize) + PD015 10 Deceleration time (Test to optimize) + PD023 1 Reverse run enabled + PD141 220 Spindle max rated voltage + PD142 3.7 Max current Amps (0.8kw=3.7 1.5kw=7.0, 2.2kw=??) + PD143 2 Poles most are 2 (I think this is only used for RPM calc from Hz) + PD144 3000 Max rated motor revolution at 50 Hz => 24000@400Hz = 3000@50HZ + + PD001 2 RS485 Control of run commands + PD002 2 RS485 Control of operating frequency + PD163 1 RS485 Address: 1 (Typical. OK to change...see below) + PD164 1 RS485 Baud rate: 9600 (Typical. OK to change...see below) + PD165 3 RS485 Mode: RTU, 8N1 The official documentation of the RS485 is horrible. I had to piece it together from - a lot of different sources + a lot of different sources: Manuals: https://github.com/RobertOlechowski/Huanyang_VFD/tree/master/Documentations/pdf Reference: https://github.com/Smoothieware/Smoothieware/blob/edge/src/modules/tools/spindle/HuanyangSpindleControl.cpp @@ -95,6 +102,17 @@ ========================================================================== + Setting registers + Addr Read Len Reg DataH DataL CRC CRC + 0x01 0x01 0x03 5 0x00 0x00 CRC CRC // PD005 + 0x01 0x01 0x03 11 0x00 0x00 CRC CRC // PD011 + 0x01 0x01 0x03 143 0x00 0x00 CRC CRC // PD143 + 0x01 0x01 0x03 144 0x00 0x00 CRC CRC // PD144 + + Message is returned with requested value = (DataH * 16) + DataL (see decimal offset above) + + ========================================================================== + Status registers Addr Read Len Reg DataH DataL CRC CRC 0x01 0x04 0x03 0x00 0x00 0x00 CRC CRC // Set Frequency * 100 (25Hz = 2500) @@ -105,19 +123,36 @@ 0x01 0x04 0x03 0x05 0x00 0x00 CRC CRC // AC voltage 0x01 0x04 0x03 0x06 0x00 0x00 CRC CRC // Cont 0x01 0x04 0x03 0x07 0x00 0x00 CRC CRC // VFD Temp + Message is returned with requested value = (DataH * 16) + DataL (see decimal offset above) + ========================================================================== + + The math: + + PD005 400 Maximum frequency Hz (Typical for spindles) + PD011 120 Min Speed (Recommend Aircooled=120 Water=100) + PD143 2 Poles most are 2 (I think this is only used for RPM calc from Hz) + PD144 3000 Max rated motor revolution at 50 Hz => 24000@400Hz = 3000@50HZ + + During initialization these 4 are pulled from the VFD registers. It then sets min and max RPM + of the spindle. So: + + MinRPM = PD011 * PD144 / 50 = 120 * 3000 / 50 = 7200 RPM min + MaxRPM = PD005 * PD144 / 50 = 400 * 3000 / 50 = 24000 RPM max + + If you then set 12000 RPM, it calculates the frequency: + + int targetFrequency = targetRPM * PD005 / MaxRPM = targetRPM * PD005 / (PD005 * PD144 / 50) = + targetRPM * 50 / PD144 = 12000 * 50 / 3000 = 200 + + If the frequency is -say- 25 Hz, Huanyang wants us to send 2500 (eg. 25.00 Hz). */ -#include - namespace Spindles { - void Huanyang::default_modbus_settings(uart_config_t& uart) { - // sets the uart to 9600 8N1 - VFD::default_modbus_settings(uart); - - // uart.baud_rate = 9600; - // Baud rate is set in the PD164 setting. + Huanyang::Huanyang() : VFD() { + // Baud rate is set in the PD164 setting. If it is not 9600, add, for example, + // _baudrate = 19200; } void Huanyang::direction_command(SpindleState mode, ModbusCommand& data) { @@ -151,13 +186,148 @@ namespace Spindles { data.msg[1] = 0x05; data.msg[2] = 0x02; - uint16_t value = (uint16_t)(rpm * 100 / 60); // send Hz * 10 (Ex:1500 RPM = 25Hz .... Send 2500) + // Frequency comes from a conversion of revolutions per second to revolutions per minute + // (factor of 60) and a factor of 2 from counting the number of poles. E.g. rpm * 120 / 100. + + // int targetFrequency = targetRPM * PD005 / MaxRPM = targetRPM * PD005 / (PD005 * PD144 / 50) = + // targetRPM * 50 / PD144 + // + // Huanyang wants a factor 100 bigger numbers. So, 1500 rpm -> 25 HZ. Send 2500. + + uint16_t value = rpm * 5000 / _maxRpmAt50Hz; data.msg[3] = (value >> 8) & 0xFF; data.msg[4] = (value & 0xFF); } - Huanyang::response_parser Huanyang::get_status_ok(ModbusCommand& data) { + VFD::response_parser Huanyang::initialization_sequence(int index, ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 6; + + // data.msg[0] is omitted (modbus address is filled in later) + data.msg[1] = 0x01; // Read setting + data.msg[2] = 0x03; // Len + // [3] = set below... + data.msg[4] = 0x00; + data.msg[5] = 0x00; + + if (index == -1) { + // Max frequency + data.msg[3] = 5; // PD005: max frequency the VFD will allow. Normally 400. + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t value = (response[4] << 8) | response[5]; +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Max frequency = %d", value); +#endif + + // Set current RPM value? Somewhere? + auto huanyang = static_cast(vfd); + huanyang->_maxFrequency = value; + return true; + }; + + } else if (index == -2) { + // Min Frequency + data.msg[3] = 11; // PD011: frequency lower limit. Normally 0. + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t value = (response[4] << 8) | response[5]; + +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Min frequency set to %d", value); +#endif + + // Set current RPM value? Somewhere? + auto huanyang = static_cast(vfd); + huanyang->_minFrequency = value; + return true; + }; + } else if (index == -3) { + // Max rated revolutions @ 50Hz + + data.msg[3] = 144; // PD144: max rated motor revolution at 50Hz => 24000@400Hz = 3000@50HZ + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t value = (response[4] << 8) | response[5]; +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Max rated revolutions @ 50Hz = %d", value); +#endif + // Set current RPM value? Somewhere? + auto huanyang = static_cast(vfd); + huanyang->_maxRpmAt50Hz = value; + + // Regarding PD144, the 2 versions of the manuals both say "This is set according to the + // actual revolution of the motor. The displayed value is the same as this set value. It + // can be used as a monitoring parameter, which is convenient to the user. This set value + // corresponds to the revolution at 50Hz". + + // Calculate the VFD settings: + huanyang->updateRPM(); + + return true; + }; + } + /* + The number of poles seems to be over constrained information with PD144. If we're wrong, here's how + to get this information. Note that you probably have to call 'updateRPM' here then: + -- + + else if (index == -4) { + // Number Poles + + data.msg[3] = 143; // PD143: 4 or 2 poles in motor. Default is 4. A spindle being 24000RPM@400Hz implies 2 poles + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint8_t value = response[4]; // Single byte response. + + // Sanity check. We expect something like 2 or 4 poles. + if (value <= 4 && value >= 2) { +#ifdef VFD_DEBUG_MODE + // Set current RPM value? Somewhere? + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Number of poles set to %d", value); +#endif + + auto huanyang = static_cast(vfd); + huanyang->_numberPoles = value; + return true; + } + else { + grbl_msg_sendf(CLIENT_ALL, + MsgLevel::Error, + "Initialization of Huanyang spindle failed. Number of poles (PD143, expected 2-4, got %d) is not sane.", + value); + return false; + } + }; + + } + */ + + // Done. + return nullptr; + } + + void Huanyang::updateRPM() { + /* + PD005 = 400 ; max frequency the VFD will allow + MaxRPM = PD005 * 50 / PD176 + + Frequencies are a factor 100 of what they should be. + */ + + if (_minFrequency > _maxFrequency) { + _minFrequency = _maxFrequency; + } + + this->_min_rpm = uint32_t(_minFrequency) * uint32_t(_maxRpmAt50Hz) / 5000; // 0 * 3000 / 50 = 0 RPM. + this->_max_rpm = uint32_t(_maxFrequency) * uint32_t(_maxRpmAt50Hz) / 5000; // 400 * 3000 / 50 = 24k RPM. + + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: VFD settings read: RPM Range(%d, %d)]", _min_rpm, _max_rpm); + } + + VFD::response_parser Huanyang::get_status_ok(ModbusCommand& data) { // NOTE: data length is excluding the CRC16 checksum. data.tx_length = 6; data.rx_length = 6; @@ -176,4 +346,42 @@ namespace Spindles { } return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { return true; }; } + + VFD::response_parser Huanyang::get_current_rpm(ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 6; + + // data.msg[0] is omitted (modbus address is filled in later) + data.msg[1] = 0x04; + data.msg[2] = 0x03; + data.msg[3] = 0x01; // Output frequency + data.msg[4] = 0x00; + data.msg[5] = 0x00; + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t frequency = (response[4] << 8) | response[5]; + + auto huanyang = static_cast(vfd); + + // Since we set a frequency, it's only fair to check if that's output in the spindle sync. + // The reason we check frequency instead of RPM is because RPM assumes a linear relation + // between RPM and frequency - which isn't necessarily true. + // + // We calculate the frequency back to RPM the same way as we calculated the frequency in the + // first place. In other words, this code must match the set_speed_command method. Note that + // we test sync_rpm instead of frequency, because that matches whatever a vfd can throw at us. + // + // value = rpm * 5000 / maxRpmAt50Hz + // + // It follows that: + // frequency_value_x100 * maxRpmAt50Hz / 5000 = rpm + + auto rpm = uint32_t(frequency) * uint32_t(huanyang->_maxRpmAt50Hz) / 5000; + + // Store RPM for synchronization + vfd->_sync_rpm = rpm; + return true; + }; + } } diff --git a/Grbl_Esp32/src/Spindles/HuanyangSpindle.h b/Grbl_Esp32/src/Spindles/HuanyangSpindle.h index 48fecfdf..e3074366 100644 --- a/Grbl_Esp32/src/Spindles/HuanyangSpindle.h +++ b/Grbl_Esp32/src/Spindles/HuanyangSpindle.h @@ -6,7 +6,7 @@ HuanyangSpindle.h Part of Grbl_ESP32 - 2020 - Bart Dring + 2020 - Bart Dring 2020 - Stefan de Bruijn Grbl is free software: you can redistribute it and/or modify @@ -28,11 +28,23 @@ namespace Spindles { int reg; protected: - void default_modbus_settings(uart_config_t& uart) override; + uint16_t _minFrequency = 0; // PD011: frequency lower limit. Normally 0. + uint16_t _maxFrequency = 400; // PD005: max frequency the VFD will allow. Normally 400. + uint16_t _maxRpmAt50Hz = 100; // PD144: rated motor revolution at 50Hz => 24000@400Hz = 3000@50HZ + // uint16_t _numberPoles = 2; // PD143: 4 or 2 poles in motor. Default is 4. A spindle being 24000RPM@400Hz implies 2 poles + + void updateRPM(); void direction_command(SpindleState mode, ModbusCommand& data) override; void set_speed_command(uint32_t rpm, ModbusCommand& data) override; + response_parser initialization_sequence(int index, ModbusCommand& data) override; response_parser get_status_ok(ModbusCommand& data) override; + response_parser get_current_rpm(ModbusCommand& data) override; + + bool supports_actual_rpm() const override { return true; } + + public: + Huanyang(); }; } diff --git a/Grbl_Esp32/src/Spindles/Laser.cpp b/Grbl_Esp32/src/Spindles/Laser.cpp index e8987112..8d5c48bf 100644 --- a/Grbl_Esp32/src/Spindles/Laser.cpp +++ b/Grbl_Esp32/src/Spindles/Laser.cpp @@ -29,14 +29,67 @@ namespace Spindles { } void Laser::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, - "Laser spindle on Pin:%s, Freq:%dHz, Res:%dbits Laser mode:%s", + "Laser spindle on Pin:%s, Enbl:%s, Freq:%dHz, Res:%dbits Laser mode:%s", _output_pin.name().c_str(), + _enable_pin.name().c_str(), int(_pwm_freq), _pwm_precision, laser_mode->getStringValue()); // the current mode use_delays = false; // this will override the value set in Spindle::PWM::init() } -} + +#if LATER + // Get the GPIO from the machine definition + void Laser::get_pins_and_settings() { + // setup all the pins + +# ifdef LASER_OUTPUT_PIN + _output_pin = LASER_OUTPUT_PIN; +# else + _output_pin = UNDEFINED_PIN; +# endif + + _invert_pwm = spindle_output_invert->get(); + +# ifdef LASER_ENABLE_PIN + _enable_pin = LASER_ENABLE_PIN; +# else + _enable_pin = UNDEFINED_PIN; +# endif + + if (_output_pin == UNDEFINED_PIN) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: LASER_OUTPUT_PIN not defined"); + return; // We cannot continue without the output pin + } + + _off_with_zero_speed = spindle_enbl_off_with_zero_speed->get(); + + _direction_pin = UNDEFINED_PIN; + is_reversable = false; + + _pwm_freq = spindle_pwm_freq->get(); + _pwm_precision = calc_pwm_precision(_pwm_freq); // detewrmine the best precision + _pwm_period = (1 << _pwm_precision); + + // pre-caculate some PWM count values + _pwm_off_value = 0; + _pwm_min_value = 0; + _pwm_max_value = _pwm_period; + + _min_rpm = 0; + _max_rpm = laser_full_power->get(); + + _piecewide_linear = false; + + _pwm_chan_num = 0; // Channel 0 is reserved for spindle use + } + + void Laser::deinit() { + stop(); + ledcDetachPin(_output_pin.getNative(Pin::Capabilities::PWM)); + _output_pin.setAttr(Pin::Attr::Input); + _enable_pin.setAttr(Pin::Attr::Input); + } diff --git a/Grbl_Esp32/src/Spindles/Laser.h b/Grbl_Esp32/src/Spindles/Laser.h index 220770b0..87673f62 100644 --- a/Grbl_Esp32/src/Spindles/Laser.h +++ b/Grbl_Esp32/src/Spindles/Laser.h @@ -36,6 +36,8 @@ namespace Spindles { bool isRateAdjusted() override; void config_message() override; + void get_pins_and_settings() override; + void deinit() override; virtual ~Laser() {} }; diff --git a/Grbl_Esp32/src/Spindles/NullSpindle.cpp b/Grbl_Esp32/src/Spindles/NullSpindle.cpp index c1266019..ddcdb7a2 100644 --- a/Grbl_Esp32/src/Spindles/NullSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/NullSpindle.cpp @@ -40,5 +40,5 @@ namespace Spindles { } SpindleState Null::get_state() { return _current_state; } void Null::stop() {} - void Null::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "No spindle"); } + void Null::config_message() { grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "No spindle"); } } diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp index 9641c7ee..d1ba9d7f 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp @@ -20,6 +20,7 @@ */ #include "PWMSpindle.h" +#include "soc/ledc_struct.h" // ======================= PWM ============================== /* @@ -34,15 +35,19 @@ namespace Spindles { get_pins_and_settings(); if (_output_pin == Pin::UNDEFINED) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin not defined"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle output pin not defined"); return; // We cannot continue without the output pin } if (!_output_pin.capabilities().has(Pin::Capabilities::PWM)) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin %s cannot do PWM", _output_pin.name().c_str()); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle output pin %s cannot do PWM", _output_pin.name().c_str()); return; } + _current_state = SpindleState::Disable; + _current_pwm_duty = 0; + use_delays = true; + auto outputNative = _output_pin.getNative(Pin::Capabilities::PWM); ledcSetup(_pwm_chan_num, (double)_pwm_freq, _pwm_precision); // setup the channel @@ -77,7 +82,7 @@ namespace Spindles { _pwm_period = (1 << _pwm_precision); if (spindle_pwm_min_value->get() > spindle_pwm_min_value->get()) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle min pwm is greater than max. Check $35 and $36"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle min pwm is greater than max. Check $35 and $36"); } // pre-caculate some PWM count values @@ -98,6 +103,9 @@ namespace Spindles { // _pwm_gradient = (_pwm_max_value - _pwm_min_value) / (_max_rpm - _min_rpm); _pwm_chan_num = 0; // Channel 0 is reserved for spindle use + + _spinup_delay = spindle_delay_spinup->get() * 1000.0; + _spindown_delay = spindle_delay_spindown->get() * 1000.0; } uint32_t PWM::set_rpm(uint32_t rpm) { @@ -107,7 +115,7 @@ namespace Spindles { return rpm; } - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "set_rpm(%d)", rpm); + //grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "set_rpm(%d)", rpm); // apply override rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (uint8_t percent) @@ -124,7 +132,7 @@ namespace Spindles { if (_piecewide_linear) { //pwm_value = piecewise_linear_fit(rpm); TODO pwm_value = 0; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Linear fit not implemented yet."); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Linear fit not implemented yet."); } else { if (rpm == 0) { @@ -134,7 +142,7 @@ namespace Spindles { } } - set_enable_pin(_current_state != SpindleState::Disable); + set_enable_pin(gc_state.modal.spindle != SpindleState::Disable); set_output(pwm_value); return 0; @@ -145,24 +153,18 @@ namespace Spindles { return; // Block during abort. } - _current_state = state; - - if (_current_state == SpindleState::Disable) { // Halt or set spindle direction and rpm. + if (state == SpindleState::Disable) { // Halt or set spindle direction and rpm. sys.spindle_speed = 0; stop(); if (use_delays && (_current_state != state)) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinDown Start "); - mc_dwell(spindle_delay_spindown->get()); - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinDown Done"); + delay(_spindown_delay); } } else { - set_dir_pin(_current_state == SpindleState::Cw); + set_dir_pin(state == SpindleState::Cw); set_rpm(rpm); - set_enable_pin(_current_state != SpindleState::Disable); // must be done after setting rpm for enable features to work + set_enable_pin(state != SpindleState::Disable); // must be done after setting rpm for enable features to work if (use_delays && (_current_state != state)) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinUp Start %d", rpm); - mc_dwell(spindle_delay_spinup->get()); - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinUp Done"); + delay(_spinup_delay); } } @@ -189,7 +191,7 @@ namespace Spindles { // prints the startup message of the spindle config void PWM::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "PWM spindle Output:%s, Enbl:%s, Dir:%s, Freq:%dHz, Res:%dbits", _output_pin.name().c_str(), @@ -215,9 +217,14 @@ namespace Spindles { duty = (1 << _pwm_precision) - duty; } - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "set_output(%d)", duty); + //ledcWrite(_pwm_chan_num, duty); - ledcWrite(_pwm_chan_num, duty); + // This was ledcWrite, but this is called from an ISR + // and ledcWrite uses RTOS features not compatible with ISRs + LEDC.channel_group[0].channel[0].duty.duty = duty << 4; + bool on = !!duty; + LEDC.channel_group[0].channel[0].conf0.sig_out_en = on; + LEDC.channel_group[0].channel[0].conf1.duty_start = on; } void PWM::set_enable_pin(bool enable) { @@ -254,4 +261,12 @@ namespace Spindles { return precision - 1; } + + void PWM::deinit() { + stop(); + ledcDetachPin(_output_pin.getNative(Pin::Capabilities::PWM)); + _output_pin.setAttr(Pin::Attr::Input); + _enable_pin.setAttr(Pin::Attr::Input); + _dir_pin.setAttr(Pin::Attr::Input); + } } diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.h b/Grbl_Esp32/src/Spindles/PWMSpindle.h index ca172523..b1bfff03 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.h +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.h @@ -68,8 +68,9 @@ namespace Spindles { virtual void set_dir_pin(bool Clockwise); virtual void set_output(uint32_t duty); virtual void set_enable_pin(bool enable_pin); + virtual void deinit(); - void get_pins_and_settings(); - uint8_t calc_pwm_precision(uint32_t freq); + virtual void get_pins_and_settings(); + uint8_t calc_pwm_precision(uint32_t freq); }; } diff --git a/Grbl_Esp32/src/Spindles/RelaySpindle.cpp b/Grbl_Esp32/src/Spindles/RelaySpindle.cpp index 62f2f1f9..4799b50a 100644 --- a/Grbl_Esp32/src/Spindles/RelaySpindle.cpp +++ b/Grbl_Esp32/src/Spindles/RelaySpindle.cpp @@ -46,7 +46,7 @@ namespace Spindles { // prints the startup message of the spindle config void Relay ::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Relay spindle Output:%s, Enbl:%s, Dir:%s", _output_pin.name().c_str(), diff --git a/Grbl_Esp32/src/Spindles/Spindle.cpp b/Grbl_Esp32/src/Spindles/Spindle.cpp index 7db5edf2..4dbd32f1 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.cpp +++ b/Grbl_Esp32/src/Spindles/Spindle.cpp @@ -100,6 +100,8 @@ namespace Spindles { protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed. set_state(state, rpm); } + + void Spindle::deinit() { stop(); } } -Spindles::Spindle* spindle; + Spindles::Spindle* spindle; diff --git a/Grbl_Esp32/src/Spindles/Spindle.h b/Grbl_Esp32/src/Spindles/Spindle.h index fefd5b06..72df58a1 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.h +++ b/Grbl_Esp32/src/Spindles/Spindle.h @@ -38,6 +38,7 @@ enum class SpindleType : int8_t { BESC, _10V, H2A, + YL620, }; #include "../Grbl.h" @@ -70,6 +71,8 @@ namespace Spindles { bool is_reversable; bool use_delays; // will SpinUp and SpinDown delays be used. volatile SpindleState _current_state = SpindleState::Disable; + uint32_t _spinup_delay; + uint32_t _spindown_delay; static void select(); diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index 1ca694c2..f002993b 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -195,12 +195,13 @@ namespace Spindles { if (retry_count == MAX_RETRIES) { if (!unresponsive) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); - if (next_cmd.critical) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); - sys_rt_exec_alarm = ExecAlarm::SpindleControl; - } unresponsive = true; } + if (next_cmd.critical) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); + mc_reset(); + sys_rt_exec_alarm = ExecAlarm::SpindleControl; + } } vTaskDelay(VFD_RS485_POLL_RATE); // TODO: What is the best value here? @@ -271,18 +272,17 @@ namespace Spindles { } // Initialization is complete, so now it's okay to run the queue task: - if (!_task_running) { // init can happen many times, we only want to start one task + task_active = true; + if (vfd_cmd_queue != nullptr) { vfd_cmd_queue = xQueueCreate(VFD_RS485_QUEUE_SIZE, sizeof(ModbusCommand)); - xTaskCreatePinnedToCore(vfd_cmd_task, // task - "vfd_cmdTaskHandle", // name for task - 2048, // size of task stack - this, // parameters - 1, // priority - &vfd_cmdTaskHandle, - 0 // core - ); - _task_running = true; } + xTaskCreatePinnedToCore(vfd_cmd_task, // task + "vfd_cmdTaskHandle", // name for task + 2048, // size of task stack + this, // parameters + 1, // priority + &vfd_cmdTaskHandle, + SUPPORT_TASK_CORE); is_reversable = true; // these VFDs are always reversable use_delays = true; @@ -327,6 +327,9 @@ namespace Spindles { _min_rpm = rpm_min->get(); _max_rpm = rpm_max->get(); + _spinup_delay = spindle_delay_spinup->get() * 1000.0; + _spindown_delay = spindle_delay_spindown->get() * 1000.0; + return pins_settings_ok; } @@ -348,17 +351,24 @@ namespace Spindles { if (_current_state != state) { // already at the desired state. This function gets called a lot. set_mode(state, critical); // critical if we are in a job + + if (rpm != 0 && (rpm < _min_rpm || rpm > _max_rpm)) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed %d outside range:(%d,%d)", rpm, _min_rpm, _max_rpm); + } + set_rpm(rpm); + + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin1"); + if (state == SpindleState::Disable) { sys.spindle_speed = 0; - if (_current_state != state) { - mc_dwell(spindle_delay_spindown->get()); - } + delay(_spindown_delay); + } else { - if (_current_state != state) { - mc_dwell(spindle_delay_spinup->get()); - } + delay(_spinup_delay); } + + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin2"); } else { if (_current_rpm != rpm) { set_rpm(rpm); @@ -432,6 +442,10 @@ namespace Spindles { set_speed_command(rpm, rpm_cmd); + // Sometimes sync_rpm is retained between different set_speed_command's. We don't want that - we want + // spindle sync to kick in after we set the speed. This forces that. + _sync_rpm = UINT32_MAX; + rpm_cmd.critical = false; if (xQueueSend(vfd_cmd_queue, &rpm_cmd, 0) != pdTRUE) { diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.h b/Grbl_Esp32/src/Spindles/VFDSpindle.h index d0ad45b0..5a293a8d 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.h +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.h @@ -2,11 +2,11 @@ /* VFDSpindle.h - + Part of Grbl_ESP32 2020 - Bart Dring 2020 - Stefan de Bruijn - + Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or @@ -22,12 +22,14 @@ #include +// #define VFD_DEBUG_MODE + namespace Spindles { class VFD : public Spindle { private: static const int VFD_RS485_MAX_MSG_SIZE = 16; // more than enough for a modbus message - static const int MAX_RETRIES = 3; // otherwise the spindle is marked 'unresponsive' + static const int MAX_RETRIES = 5; // otherwise the spindle is marked 'unresponsive' bool set_mode(SpindleState mode, bool critical); bool get_pins_and_settings(); @@ -64,10 +66,12 @@ namespace Spindles { // Commands that return the status. Returns nullptr if unavailable by this VFD (default): using response_parser = bool (*)(const uint8_t* response, VFD* spindle); - virtual response_parser get_max_rpm(ModbusCommand& data) { return nullptr; } + virtual response_parser initialization_sequence(int index, ModbusCommand& data) { return nullptr; } virtual response_parser get_current_rpm(ModbusCommand& data) { return nullptr; } virtual response_parser get_current_direction(ModbusCommand& data) { return nullptr; } virtual response_parser get_status_ok(ModbusCommand& data) = 0; + virtual bool supports_actual_rpm() const { return false; } + virtual bool safety_polling() const { return true; } public: VFD() = default; @@ -80,6 +84,8 @@ namespace Spindles { // Should hide them and use a member function. volatile uint32_t _min_rpm; volatile uint32_t _max_rpm; + volatile uint32_t _sync_rpm; + volatile bool _syncing; void init(); void config_message(); diff --git a/Grbl_Esp32/src/Spindles/YL620Spindle.cpp b/Grbl_Esp32/src/Spindles/YL620Spindle.cpp new file mode 100644 index 00000000..68383cad --- /dev/null +++ b/Grbl_Esp32/src/Spindles/YL620Spindle.cpp @@ -0,0 +1,232 @@ +#include "YL620Spindle.h" + +/* + YL620Spindle.cpp + + This is for a Yalang YL620/YL620-A VFD based spindle to be controlled via RS485 Modbus RTU. + + Part of Grbl_ESP32 + 2021 - Marco Wagner + + 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 . + + WARNING!!!! + VFDs are very dangerous. They have high voltages and are very powerful + Remove power before changing bits. + + ============================================================================================================= + + Configuration required for the YL620 + + Parameter number Description Value + ------------------------------------------------------------------------------- + P00.00 Main frequency 400.00Hz (match to your spindle) + P00.01 Command source 3 + + P03.00 RS485 Baud rate 3 (9600) + P03.01 RS485 address 1 + P03.02 RS485 protocol 2 + P03.08 Frequency given lower limit 100.0Hz (match to your spindle cooling-type) + + =============================================================================================================== + + RS485 communication is standard Modbus RTU + + Therefore, the following operation codes are relevant: + 0x03: read single holding register + 0x06: write single holding register + + Holding register address Description + --------------------------------------------------------------------------- + 0x0000 main frequency + 0x0308 frequency given lower limit + + 0x2000 command register (further information below) + 0x2001 Modbus485 frequency command (x0.1Hz => 2500 = 250.0Hz) + + 0x200A Target frequency + 0x200B Output frequency + 0x200C Output current + + + Command register at holding address 0x2000 + -------------------------------------------------------------------------- + bit 1:0 b00: No function + b01: shutdown command + b10: start command + b11: Jog command + bit 3:2 reserved + bit 5:4 b00: No function + b01: Forward command + b10: Reverse command + b11: change direction + bit 7:6 b00: No function + b01: reset an error flag + b10: reset all error flags + b11: reserved +*/ + +namespace Spindles { + YL620::YL620() : VFD() {} + + void YL620::direction_command(SpindleState mode, ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 6; + + // data.msg[0] is omitted (modbus address is filled in later) + data.msg[1] = 0x06; // 06: write output register + data.msg[2] = 0x20; // 0x2000: command register address + data.msg[3] = 0x00; + + data.msg[4] = 0x00; // High-Byte of command always 0x00 + switch (mode) { + case SpindleState::Cw: + data.msg[5] = 0x12; // Start in forward direction + break; + case SpindleState::Ccw: + data.msg[5] = 0x22; // Start in reverse direction + break; + default: // SpindleState::Disable + data.msg[5] = 0x01; // Disable spindle + break; + } + } + + void YL620::set_speed_command(uint32_t rpm, ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 6; + + // We have to know the max RPM before we can set the current RPM: + auto max_rpm = this->_max_rpm; + auto max_frequency = this->_maxFrequency; + + uint16_t freqFromRPM = (uint16_t(rpm) * uint16_t(max_frequency)) / uint16_t(max_rpm); + +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "For %d RPM the output frequency is set to %d Hz*10", int(rpm), int(freqFromRPM)); +#endif + + data.msg[1] = 0x06; + data.msg[2] = 0x20; + data.msg[3] = 0x01; + data.msg[4] = uint8_t(freqFromRPM >> 8); + data.msg[5] = uint8_t(freqFromRPM & 0xFF); + } + + VFD::response_parser YL620::initialization_sequence(int index, ModbusCommand& data) { + if (index == -1) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 5; + + data.msg[1] = 0x03; + data.msg[2] = 0x03; + data.msg[3] = 0x08; + data.msg[4] = 0x00; + data.msg[5] = 0x01; + + // Recv: 01 03 02 03 E8 xx xx + // -- -- = 1000 + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + auto yl620 = static_cast(vfd); + yl620->_minFrequency = (uint16_t(response[3]) << 8) | uint16_t(response[4]); + +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "YL620 allows minimum frequency of %d Hz", int(yl620->_minFrequency)); +#endif + + return true; + }; + } else if (index == -2) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 5; + + data.msg[1] = 0x03; + data.msg[2] = 0x00; + data.msg[3] = 0x00; + data.msg[4] = 0x00; + data.msg[5] = 0x01; + + // Recv: 01 03 02 0F A0 xx xx + // -- -- = 4000 + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + auto yl620 = static_cast(vfd); + yl620->_maxFrequency = (uint16_t(response[3]) << 8) | uint16_t(response[4]); + + vfd->_min_rpm = uint32_t(yl620->_minFrequency) * uint32_t(vfd->_max_rpm) / + uint32_t(yl620->_maxFrequency); // 1000 * 24000 / 4000 = 6000 RPM. + +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "YL620 allows maximum frequency of %d Hz", int(yl620->_maxFrequency)); + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "Configured maxRPM of %d RPM results in minRPM of %d RPM", + int(vfd->_max_rpm), + int(vfd->_min_rpm)); +#endif + + return true; + }; + } else { + return nullptr; + } + } + + VFD::response_parser YL620::get_current_rpm(ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 5; + + // Send: 01 03 200B 0001 + data.msg[1] = 0x03; + data.msg[2] = 0x20; + data.msg[3] = 0x0B; + data.msg[4] = 0x00; + data.msg[5] = 0x01; + + // Recv: 01 03 02 05 DC xx xx + // ---- = 1500 + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t freq = (uint16_t(response[3]) << 8) | uint16_t(response[4]); + + auto yl620 = static_cast(vfd); + + uint16_t rpm = freq * uint16_t(vfd->_max_rpm) / uint16_t(yl620->_maxFrequency); + + // Set current RPM value? Somewhere? + vfd->_sync_rpm = rpm; + return true; + }; + } + + VFD::response_parser YL620::get_current_direction(ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 5; + + // Send: 01 03 20 00 00 01 + data.msg[1] = 0x03; + data.msg[2] = 0x20; + data.msg[3] = 0x00; + data.msg[4] = 0x00; + data.msg[5] = 0x01; + + // Receive: 01 03 02 00 0A xx xx + // ----- status is in 00 0A bit 5:4 + + // TODO: What are we going to do with this? Update sys.spindle_speed? Update vfd state? + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { return true; }; + } +} diff --git a/Grbl_Esp32/src/Spindles/YL620Spindle.h b/Grbl_Esp32/src/Spindles/YL620Spindle.h new file mode 100644 index 00000000..d2d2e10b --- /dev/null +++ b/Grbl_Esp32/src/Spindles/YL620Spindle.h @@ -0,0 +1,44 @@ +#pragma once + +#include "VFDSpindle.h" + +/* + YL620Spindle.h + + Part of Grbl_ESP32 + 2021 - Marco Wagner + + 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 . + +*/ + +namespace Spindles { + class YL620 : public VFD { + protected: + uint16_t _minFrequency = 0; // frequency lower limit. Factor 10 of actual frequency + uint16_t _maxFrequency = 4000; // max frequency the VFD will allow. Normally 400.0. Factor 10 of actual frequency + + void direction_command(SpindleState mode, ModbusCommand& data) override; + void set_speed_command(uint32_t rpm, ModbusCommand& data) override; + + response_parser initialization_sequence(int index, ModbusCommand& data) override; + response_parser get_current_rpm(ModbusCommand& data) override; + response_parser get_current_direction(ModbusCommand& data) override; + response_parser get_status_ok(ModbusCommand& data) override { return nullptr; } + + bool supports_actual_rpm() const override { return true; } + bool safety_polling() const override { return false; } + + public: + YL620(); + }; +} diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index 149acc88..d8e8fb54 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -25,6 +25,7 @@ #include "Grbl.h" #include "MachineConfig.h" +#include // Stores the planner block Bresenham algorithm execution data for the segments in the segment // buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will @@ -59,10 +60,7 @@ typedef struct { uint32_t counter[MAX_N_AXIS]; // Counter variables for the bresenham line tracer -#ifdef STEP_PULSE_DELAY - uint8_t step_bits; // Stores out_bits output to complete the step pulse delay -#endif - + uint8_t step_bits; // Stores out_bits output to complete the step pulse delay uint8_t execute_step; // Flags step execution for each interrupt. uint8_t step_pulse_time; // Step pulse reset time after step rise uint8_t step_outbits; // The next stepping-bits to be output @@ -82,7 +80,7 @@ static uint8_t segment_buffer_head; static uint8_t segment_next_head; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static volatile uint8_t busy; +static std::atomic busy; // Pointers for the step segment being prepped from the planner buffer. Accessed only by the // main program. Pointers may be planning segments or planner blocks ahead of what being executed. @@ -183,7 +181,7 @@ stepper_id_t current_stepper = DEFAULT_STEPPER; The complete step timing should look this... Direction pin is set - An optional (via STEP_PULSE_DELAY in config.h) is put after this + An optional delay (direction_delay_microseconds) is put after this The step pin is started A pulse length is determine (via option $0 ... pulse_microseconds) The pulse is ended @@ -197,19 +195,21 @@ 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 ======================================================================================= - //const int timer_idx = (int)para; // get the timer index +void IRAM_ATTR onStepperDriverTimer(void* para) { + // Timer ISR, normally takes a step. + // + // When handling an interrupt within an interrupt serivce routine (ISR), the interrupt status bit + // needs to be explicitly cleared. TIMERG0.int_clr_timers.t0 = 1; - if (busy) { - return; // The busy-flag is used to avoid reentering this interrupt + + bool expected = false; + if (busy.compare_exchange_strong(expected, true)) { + stepper_pulse_func(); + + TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN; + + busy.store(false); } - busy = true; - - stepper_pulse_func(); - - TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN; - busy = false; } /** @@ -222,7 +222,35 @@ void IRAM_ATTR onStepperDriverTimer( static void stepper_pulse_func() { auto n_axis = MachineConfig::instance()->_axes->_numberAxis; - MachineConfig::instance()->_axes->step(st.step_outbits, st.dir_outbits); +#ifdef LATER + // XXX this should be in the motor driver, not here + if (motors_direction(st.dir_outbits)) { + auto wait_direction = direction_delay_microseconds->get(); + if (wait_direction > 0) { + // Stepper drivers need some time between changing direction and doing a pulse. + switch (current_stepper) { + case ST_I2S_STREAM: + i2s_out_push_sample(wait_direction); + break; + case ST_I2S_STATIC: + case ST_TIMED: { + // wait for step pulse time to complete...some time expired during code above + // + // If we are using GPIO stepping as opposed to RMT, record the + // time that we turned on the direction pins so we can delay a bit. + // If we are using RMT, we can't delay here. + auto direction_pulse_start_time = esp_timer_get_time() + wait_direction; + while ((esp_timer_get_time() - direction_pulse_start_time) < 0) { + NOP(); // spin here until time to turn off step + } + break; + } + case ST_RMT: + break; + } + } + } +#endif // If we are using GPIO stepping as opposed to RMT, record the // time that we turned on the step pins so we can turn them off @@ -230,6 +258,7 @@ static void stepper_pulse_func() { // This is unnecessary with RMT and I2S stepping since both of // those methods time the turn off automatically. uint64_t step_pulse_start_time = esp_timer_get_time(); + MachineConfig::instance()->_axes->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) { @@ -246,11 +275,9 @@ static void stepper_pulse_func() { st.exec_block_index = st.exec_segment->st_block_index; st.exec_block = &st_block_buffer[st.exec_block_index]; // Initialize Bresenham line and distance counters - // XXX the original code only inits X, Y, Z here, instead of n_axis. Is that correct? - for (int axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < n_axis; axis++) { st.counter[axis] = (st.exec_block->step_event_count >> 1); } - // TODO ABC } st.dir_outbits = st.exec_block->direction_bits; // Adjust Bresenham axis increment counters according to AMASS level. @@ -326,6 +353,8 @@ static void stepper_pulse_func() { } void stepper_init() { + busy.store(false); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Axis count %d", MachineConfig::instance()->_axes->_numberAxis); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s", stepper_names[current_stepper]); @@ -363,12 +392,17 @@ void st_wake_up() { MachineConfig::instance()->_axes->set_disable(false); stepper_idle = false; // Initialize step pulse timing from settings. Here to ensure updating after re-writing. -#ifdef STEP_PULSE_DELAY +#ifdef USE_RMT_STEPS // Step pulse delay handling is not require with ESP32...the RMT function does it. + if (direction_delay_microseconds->get() < 1) { + // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement. + st.step_pulse_time = -(((pulse_microseconds->get() - 2) * ticksPerMicrosecond) >> 3); + } #else // Normal operation // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement. st.step_pulse_time = -(((pulse_microseconds->get() - 2) * ticksPerMicrosecond) >> 3); #endif + // Enable Stepper Driver Interrupt Stepper_Timer_Start(); } @@ -393,7 +427,6 @@ void st_reset() { segment_buffer_tail = 0; segment_buffer_head = 0; // empty = tail segment_next_head = 1; - busy = false; st.step_outbits = 0; st.dir_outbits = 0; // Initialize direction bits to default. // TODO do we need to turn step pins off? @@ -403,7 +436,6 @@ void st_reset() { void st_go_idle() { // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active. Stepper_Timer_Stop(); - busy = false; // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances. if (((stepper_idle_lock_time->get() != 0xff) || sys_rt_exec_alarm != ExecAlarm::None || sys.state == State::Sleep) && @@ -554,7 +586,9 @@ void st_prep_buffer() { prep.current_speed = sqrt(pl_block->entry_speed_sqr); } - if (spindle->isRateAdjusted()) { // MachineConfig::instance()->_laserMode { + st_prep_block->is_pwm_rate_adjusted = false; // set default value + // prep.inv_rate is only used if is_pwm_rate_adjusted is true + if (MachineConfig::instance()->_laserMode) { if (pl_block->spindle == SpindleState::Ccw) { // Pre-compute inverse programmed rate to speed up PWM updating per step segment. prep.inv_rate = 1.0 / pl_block->programmed_rate; diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index cc42ae30..a9399d73 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -32,6 +32,7 @@ volatile ExecState sys_rt_exec_state; // Global realtime executor bitflag v volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. volatile ExecAccessory sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. volatile bool cycle_stop; // For state transitions, instead of bitflag +volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while cartesian_to_motors has taken ownership of a line motion #ifdef DEBUG volatile bool sys_rt_exec_debug; #endif @@ -49,6 +50,7 @@ void system_ini() { // Renamed from system_init() due to conflict with esp32 fi // setup control inputs if (ControlSafetyDoorPin->get() != Pin::UNDEFINED) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Door switch on pin %s", pinName(CONTROL_SAFETY_DOOR_PIN).c_str()); auto pin = ControlSafetyDoorPin->get(); auto attr = Pin::Attr::Input | Pin::Attr::ISR; if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) { @@ -59,6 +61,7 @@ void system_ini() { // Renamed from system_init() due to conflict with esp32 fi } if (ControlResetPin->get() != Pin::UNDEFINED) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Reset switch on pin %s", pinName(CONTROL_RESET_PIN).c_str()); auto pin = ControlResetPin->get(); auto attr = Pin::Attr::Input | Pin::Attr::ISR; if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) { @@ -69,6 +72,7 @@ void system_ini() { // Renamed from system_init() due to conflict with esp32 fi } if (ControlFeedHoldPin->get() != Pin::UNDEFINED) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Hold switch on pin %s", pinName(CONTROL_FEED_HOLD_PIN).c_str()); auto pin = ControlFeedHoldPin->get(); auto attr = Pin::Attr::Input | Pin::Attr::ISR; if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) { @@ -79,6 +83,7 @@ void system_ini() { // Renamed from system_init() due to conflict with esp32 fi } if (ControlCycleStartPin->get() != Pin::UNDEFINED) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start switch on pin %s", pinName(CONTROL_CYCLE_START_PIN).c_str()); auto pin = ControlCycleStartPin->get(); auto attr = Pin::Attr::Input | Pin::Attr::ISR; if (pin.capabilities().has(Pins::PinCapabilities::PullUp)) { @@ -133,7 +138,7 @@ void system_ini() { // Renamed from system_init() due to conflict with esp32 fi control_sw_queue = xQueueCreate(10, sizeof(int)); xTaskCreate(controlCheckTask, "controlCheckTask", - 2048, + 3096, NULL, 5, // priority NULL); @@ -170,7 +175,9 @@ void controlCheckTask(void* pvParameters) { debouncing = false; static UBaseType_t uxHighWaterMark = 0; +# ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +# endif } } #endif @@ -205,9 +212,6 @@ void system_flag_wco_change() { sys.report_wco_counter = 0; } -// Returns machine position of axis 'idx'. Must be sent a 'step' array. -// NOTE: If motor steps and machine position are not in the same coordinate frame, this function -// 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(); @@ -216,14 +220,20 @@ float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx) { } void system_convert_array_steps_to_mpos(float* position, int32_t* steps) { - uint8_t idx; - auto n_axis = MachineConfig::instance()->_axes->_numberAxis; - for (idx = 0; idx < n_axis; idx++) { - position[idx] = system_convert_axis_steps_to_mpos(steps, idx); + auto n_axis = number_axis->get(); + float motors[n_axis]; + for (int idx = 0; idx < n_axis; idx++) { + motors[idx] = (float)steps[idx] / axis_settings[idx]->steps_per_mm->get(); } - return; + motors_to_cartesian(position, motors, n_axis); } +float* system_get_mpos() { + static float position[MAX_N_AXIS]; + system_convert_array_steps_to_mpos(position, sys_position); + return position; +}; + // Returns control pin state as a uint8 bitfield. Each bit indicates the input pin state, where // triggered is 1 and not triggered is 0. Invert mask is applied. Bitfield organization is // defined by the ControlPin in System.h. @@ -302,36 +312,29 @@ void system_exec_control_pin(ControlPins pins) { } } -// io_num is the virtual pin# and has nothing to do with the actual esp32 GPIO_NUM_xx -// It uses a mask so all can be turned of in ms_reset -bool sys_io_control(uint8_t io_num_mask, bool turnOn, bool synchronized) { - bool cmd_ok = true; - if (synchronized) - protocol_buffer_synchronize(); - +void sys_digital_all_off() { for (uint8_t io_num = 0; io_num < MaxUserDigitalPin; io_num++) { - if (io_num_mask & bit(io_num)) { - if (!myDigitalOutputs[io_num]->set_level(turnOn)) - cmd_ok = false; - } + myDigitalOutputs[io_num]->set_level(LOW); } - return cmd_ok; } -// io_num is the virtual pin# and has nothing to do with the actual esp32 GPIO_NUM_xx -// It uses a mask so all can be turned of in ms_reset -bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized) { - bool cmd_ok = true; - if (synchronized) - protocol_buffer_synchronize(); +// io_num is the virtual digital pin# +bool sys_set_digital(uint8_t io_num, bool turnOn) { + return myDigitalOutputs[io_num]->set_level(turnOn); +} +// Turn off all analog outputs +void sys_analog_all_off() { for (uint8_t io_num = 0; io_num < MaxUserDigitalPin; io_num++) { - if (io_num_mask & bit(io_num)) { - if (!myAnalogOutputs[io_num]->set_level(duty)) - cmd_ok = false; - } + myAnalogOutputs[io_num]->set_level(0); } - return cmd_ok; +} + +// io_num is the virtual analog pin# +bool sys_set_analog(uint8_t io_num, float percent) { + auto analog = myAnalogOutputs[io_num]; + uint32_t numerator = percent / 100.0 * analog->denominator(); + return analog->set_level(numerator); } /* @@ -369,4 +372,40 @@ uint8_t sys_calc_pwm_precision(uint32_t freq) { return precision - 1; } -void __attribute__((weak)) user_defined_macro(uint8_t index); + +void __attribute__((weak)) user_defined_macro(uint8_t index) { + // must be in Idle + if (sys.state != State::Idle) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro button only permitted in idle"); + return; + } + + String user_macro; + char line[255]; + switch (index) { + case 0: + user_macro = user_macro0->get(); + break; + case 1: + user_macro = user_macro1->get(); + break; + case 2: + user_macro = user_macro2->get(); + break; + case 3: + user_macro = user_macro3->get(); + break; + default: + return; + } + + if (user_macro == "") { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro User/Macro%d empty", index); + return; + } + + user_macro.replace('&', '\n'); + user_macro.toCharArray(line, 255, 0); + strcat(line, "\r"); + WebUI::inputBuffer.push(line); +} diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index 0e92eff7..3562afa8 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -142,6 +142,7 @@ extern volatile Percent sys_rt_f_override; // Feed override extern volatile Percent sys_rt_r_override; // Rapid feed override value in percent extern volatile Percent sys_rt_s_override; // Spindle override value in percent extern volatile bool cycle_stop; +extern volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while cartesian_to_motors has taken ownership of a line motion #ifdef DEBUG extern volatile bool sys_rt_exec_debug; #endif @@ -168,14 +169,16 @@ void system_flag_wco_change(); float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx); // Updates a machine 'position' array based on the 'step' array sent. -void system_convert_array_steps_to_mpos(float* position, int32_t* steps); +void system_convert_array_steps_to_mpos(float* position, int32_t* steps); +float* system_get_mpos(); // A task that runs after a control switch interrupt for debouncing. void controlCheckTask(void* pvParameters); void system_exec_control_pin(ControlPins pins); -bool sys_io_control(uint8_t io_num_mask, bool turnOn, bool synchronized); -bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized); +bool sys_set_digital(uint8_t io_num, bool turnOn); +void sys_digital_all_off(); +bool sys_set_analog(uint8_t io_num, float percent); +void sys_analog_all_off(); -int8_t sys_get_next_PWM_chan_num(); -uint8_t sys_calc_pwm_precision(uint32_t freq); +int8_t sys_get_next_PWM_chan_num(); diff --git a/Grbl_Esp32/src/Uart.cpp b/Grbl_Esp32/src/Uart.cpp new file mode 100644 index 00000000..7bb08382 --- /dev/null +++ b/Grbl_Esp32/src/Uart.cpp @@ -0,0 +1,94 @@ +/* + * UART driver that accesses the ESP32 hardware FIFOs directly. + */ + +#include "Grbl.h" + +#include "esp_system.h" +#include "soc/uart_reg.h" +#include "soc/io_mux_reg.h" +#include "soc/gpio_sig_map.h" +#include "soc/dport_reg.h" +#include "soc/rtc.h" + +Uart::Uart(int uart_num) : _uart_num(uart_port_t(uart_num)), _pushback(-1) {} + +void Uart::begin(unsigned long baudrate, Data dataBits, Stop stopBits, Parity parity) { + // uart_driver_delete(_uart_num); + uart_config_t conf; + conf.baud_rate = baudrate; + conf.data_bits = uart_word_length_t(dataBits); + conf.parity = uart_parity_t(parity); + conf.stop_bits = uart_stop_bits_t(stopBits); + conf.flow_ctrl = UART_HW_FLOWCTRL_DISABLE; + conf.rx_flow_ctrl_thresh = 0; + conf.use_ref_tick = false; + if (uart_param_config(_uart_num, &conf) != ESP_OK) { + return; + }; + uart_driver_install(_uart_num, 256, 0, 0, NULL, 0); +} + +int Uart::available() { + size_t size = 0; + uart_get_buffered_data_len(_uart_num, &size); + return size + (_pushback >= 0); +} + +int Uart::peek() { + _pushback = read(); + return _pushback; +} + +int Uart::read(TickType_t timeout) { + if (_pushback >= 0) { + int ret = _pushback; + _pushback = -1; + return ret; + } + uint8_t c; + int res = uart_read_bytes(_uart_num, &c, 1, timeout); + return res != 1 ? -1 : c; +} +int Uart::read() { + return read(0); +} + +size_t Uart::readBytes(char* buffer, size_t length, TickType_t timeout) { + bool pushback = _pushback >= 0; + if (pushback && length) { + *buffer++ = _pushback; + _pushback = -1; + --length; + } + int res = uart_read_bytes(_uart_num, (uint8_t*)buffer, length, timeout); + // The Stream class version of readBytes never returns -1, + // so if uart_read_bytes returns -1, we change that to 0 + return pushback + (res >= 0 ? res : 0); +} +size_t Uart::readBytes(char* buffer, size_t length) { + return readBytes(buffer, length, (TickType_t)0); +} +size_t Uart::write(uint8_t c) { + return uart_write_bytes(_uart_num, (char*)&c, 1); +} + +size_t Uart::write(const uint8_t* buffer, size_t length) { + return uart_write_bytes(_uart_num, (const char*)buffer, length); +} + +size_t Uart::write(const char* text) { + return uart_write_bytes(_uart_num, text, strlen(text)); +} + +bool Uart::setHalfDuplex() { + return uart_set_mode(_uart_num, UART_MODE_RS485_HALF_DUPLEX) != ESP_OK; +} +bool Uart::setPins(int tx_pin, int rx_pin, int rts_pin, int cts_pin) { + return uart_set_pin(_uart_num, tx_pin, rx_pin, rts_pin, cts_pin) != ESP_OK; +} +bool Uart::flushTxTimed(TickType_t ticks) { + return uart_wait_tx_done(_uart_num, ticks) != ESP_OK; +} + +Uart Uart0(0); diff --git a/Grbl_Esp32/src/Uart.h b/Grbl_Esp32/src/Uart.h new file mode 100644 index 00000000..e942b161 --- /dev/null +++ b/Grbl_Esp32/src/Uart.h @@ -0,0 +1,49 @@ +#pragma once + +#include + +class Uart : public Stream { +private: + uart_port_t _uart_num; + int _pushback; + +public: + enum class Data : int { + Bits5 = UART_DATA_5_BITS, + Bits6 = UART_DATA_6_BITS, + Bits7 = UART_DATA_7_BITS, + Bits8 = UART_DATA_8_BITS, + }; + + enum class Stop : int { + Bits1 = UART_STOP_BITS_1, + Bits1_5 = UART_STOP_BITS_1_5, + Bits2 = UART_STOP_BITS_2, + }; + + enum class Parity : int { + None = UART_PARITY_DISABLE, + Even = UART_PARITY_EVEN, + Odd = UART_PARITY_ODD, + }; + + Uart(int uart_num); + bool setHalfDuplex(); + bool setPins(int tx_pin, int rx_pin, int rts_pin = -1, int cts_pin = -1); + void begin(unsigned long baud, Data dataBits, Stop stopBits, Parity parity); + int available(void) override; + int read(void) override; + int read(TickType_t timeout); + size_t readBytes(char* buffer, size_t length, TickType_t timeout); + size_t readBytes(uint8_t* buffer, size_t length, TickType_t timeout) { return readBytes((char*)buffer, length, timeout); } + size_t readBytes(char* buffer, size_t length) override; + int peek(void) override; + size_t write(uint8_t data); + size_t write(const uint8_t* buffer, size_t length); + inline size_t write(const char* buffer, size_t size) { return write((uint8_t*)buffer, size); } + size_t write(const char* text); + void flush() { uart_flush(_uart_num); } + bool flushTxTimed(TickType_t ticks); +}; + +extern Uart Uart0; diff --git a/Grbl_Esp32/src/UserOutput.cpp b/Grbl_Esp32/src/UserOutput.cpp index bea88cbd..1a5dead9 100644 --- a/Grbl_Esp32/src/UserOutput.cpp +++ b/Grbl_Esp32/src/UserOutput.cpp @@ -66,8 +66,16 @@ namespace UserOutput { return; } - // determine the highest bit precision allowed by frequency - _resolution_bits = sys_calc_pwm_precision(_pwm_frequency); + // determine the highest resolution (number of precision bits) allowed by frequency + uint32_t apb_frequency = getApbFrequency(); + + // increase the precision (bits) until it exceeds allow by frequency the max or is 16 + _resolution_bits = 0; + while ((1 << _resolution_bits) < (apb_frequency / _pwm_frequency) && _resolution_bits <= 16) { + ++_resolution_bits; + } + // _resolution_bits is now just barely too high, so drop it down one + --_resolution_bits; init(); } @@ -94,9 +102,7 @@ namespace UserOutput { } // returns true if able to set value - bool AnalogOutput::set_level(float percent) { - float duty; - + bool AnalogOutput::set_level(uint32_t numerator) { // look for errors, but ignore if turning off to prevent mask turn off from generating errors if (_pin == Pin::UNDEFINED) { return false; @@ -107,15 +113,13 @@ namespace UserOutput { return false; } - if (_current_value == percent) { + if (_current_value == numerator) { return true; } - _current_value = percent; + _current_value = numerator; - duty = (percent / 100.0) * (1 << _resolution_bits); - - ledcWrite(_pwm_channel, duty); + ledcWrite(_pwm_channel, numerator); return true; } diff --git a/Grbl_Esp32/src/UserOutput.h b/Grbl_Esp32/src/UserOutput.h index 637497fa..28c1d15b 100644 --- a/Grbl_Esp32/src/UserOutput.h +++ b/Grbl_Esp32/src/UserOutput.h @@ -43,17 +43,18 @@ namespace UserOutput { public: AnalogOutput(); AnalogOutput(uint8_t number, Pin pin, float pwm_frequency); - bool set_level(float percent); + bool set_level(uint32_t numerator); + uint32_t denominator() { return 1UL << _resolution_bits; }; protected: void init(); void config_message(); - uint8_t _number = UNDEFINED_OUTPUT; - Pin _pin; - uint8_t _pwm_channel = -1; // -1 means invalid or not setup - float _pwm_frequency; - uint8_t _resolution_bits; - float _current_value; + uint8_t _number = UNDEFINED_OUTPUT; + Pin _pin; + uint8_t _pwm_channel = -1; // -1 means invalid or not setup + float _pwm_frequency; + uint8_t _resolution_bits; + uint32_t _current_value; }; } diff --git a/Grbl_Esp32/src/WebUI/BTConfig.cpp b/Grbl_Esp32/src/WebUI/BTConfig.cpp index 05a7816e..85a2a7e6 100644 --- a/Grbl_Esp32/src/WebUI/BTConfig.cpp +++ b/Grbl_Esp32/src/WebUI/BTConfig.cpp @@ -86,6 +86,9 @@ namespace WebUI { bool BTConfig::isBTnameValid(const char* hostname) { //limited size + if (!hostname) { + return true; + } char c; // length is checked automatically by string setting //only letter and digit diff --git a/Grbl_Esp32/src/WebUI/Commands.cpp b/Grbl_Esp32/src/WebUI/Commands.cpp index 7cfdf003..d59ee19c 100644 --- a/Grbl_Esp32/src/WebUI/Commands.cpp +++ b/Grbl_Esp32/src/WebUI/Commands.cpp @@ -43,6 +43,9 @@ namespace WebUI { } bool COMMANDS::isLocalPasswordValid(char* password) { + if (!password) { + return true; + } char c; //limited size if ((strlen(password) > MAX_LOCAL_PASSWORD_LENGTH) || (strlen(password) < MIN_LOCAL_PASSWORD_LENGTH)) { diff --git a/Grbl_Esp32/src/WebUI/TelnetServer.cpp b/Grbl_Esp32/src/WebUI/TelnetServer.cpp index 0829bddb..b5cfaa1e 100644 --- a/Grbl_Esp32/src/WebUI/TelnetServer.cpp +++ b/Grbl_Esp32/src/WebUI/TelnetServer.cpp @@ -208,11 +208,11 @@ namespace WebUI { if (current > (TELNETRXBUFFERSIZE - 1)) { current = 0; } - if (char(data[i]) != '\r') { - _RXbuffer[current] = data[i]; - current++; - data_processed++; - } + + _RXbuffer[current] = data[i]; + current++; + data_processed++; + COMMANDS::wait(0); //vTaskDelay(1 / portTICK_RATE_MS); // Yield to other tasks } diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index f9ef698d..1a3f2e99 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -280,7 +280,14 @@ namespace WebUI { if ((path.substring(0, 4) == "/SD/")) { //remove /SD path = path.substring(3); + if (SDState::Idle != get_sd_state(true)) { + String content = "cannot open: "; + content += path + ", SD is not available."; + + _webserver->send(500, "text/plain", content); + } if (SD.exists(pathWithGz) || SD.exists(path)) { + set_sd_state(SDState::BusyUploading); if (SD.exists(pathWithGz)) { path = pathWithGz; } @@ -311,8 +318,10 @@ namespace WebUI { if (i != totalFileSize) { //error: TBD } + set_sd_state(SDState::Idle); return; } + set_sd_state(SDState::Idle); } String content = "cannot find "; content += path; @@ -471,10 +480,11 @@ namespace WebUI { } } if (silent || !espresponse->anyOutput()) { - _webserver->send(err != Error::Ok ? 401 : 200, "text/plain", answer); + _webserver->send(err != Error::Ok ? 500 : 200, "text/plain", answer); } else { espresponse->flush(); } + if(espresponse) delete(espresponse); } else { //execute GCODE if (auth_level == AuthenticationLevel::LEVEL_GUEST) { _webserver->send(401, "text/plain", "Authentication failed!\n"); @@ -482,7 +492,7 @@ namespace WebUI { } //Instead of send several commands one by one by web / send full set and split here String scmd; - const char* res = ""; + bool hasError =false; uint8_t sindex = 0; // TODO Settings - this is very inefficient. get_Splited_Value() is O(n^2) // when it could easily be O(n). Also, it would be just as easy to push @@ -503,10 +513,10 @@ namespace WebUI { scmd += "\n"; } if (!Serial2Socket.push(scmd.c_str())) { - res = "Error"; + hasError = true; } } - _webserver->send(200, "text/plain", res); + _webserver->send(200, "text/plain", hasError?"Error":""); } } @@ -878,7 +888,6 @@ namespace WebUI { path = ""; _webserver->sendHeader("Cache-Control", "no-cache"); _webserver->send(200, "application/json", jsonfile); - _upload_status = UploadStatusType::NONE; } //push error code and message to websocket @@ -1216,17 +1225,20 @@ namespace WebUI { String sstatus = "Ok"; if ((_upload_status == UploadStatusType::FAILED) || (_upload_status == UploadStatusType::FAILED)) { sstatus = "Upload failed"; - _upload_status = UploadStatusType::NONE; } + _upload_status = UploadStatusType::NONE; bool list_files = true; uint64_t totalspace = 0; uint64_t usedspace = 0; - if (get_sd_state(true) != SDCARD_IDLE) { + SDState state = get_sd_state(true); + if (state != SDState::Idle) { + String status = "{\"status\":\""; + status += state == SDState::NotPresent ? "No SD Card\"}" : "Busy\"}"; _webserver->sendHeader("Cache-Control", "no-cache"); - _webserver->send(200, "application/json", "{\"status\":\"No SD Card\"}"); + _webserver->send(200, "application/json", status); return; } - set_sd_state(SDCARD_BUSY_PARSING); + set_sd_state(SDState::BusyParsing); //get current path if (_webserver->hasArg("path")) { @@ -1319,6 +1331,7 @@ namespace WebUI { s += " does not exist on SD Card\"}"; _webserver->send(200, "application/json", s); SD.end(); + set_sd_state(SDState::Idle); return; } if (list_files) { @@ -1395,8 +1408,7 @@ namespace WebUI { jsonfile += "}"; _webserver->sendHeader("Cache-Control", "no-cache"); _webserver->send(200, "application/json", jsonfile); - _upload_status = UploadStatusType::NONE; - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); SD.end(); } @@ -1423,13 +1435,13 @@ namespace WebUI { filename = "/" + upload.filename; } //check if SD Card is available - if (get_sd_state(true) != SDCARD_IDLE) { + if (get_sd_state(true) != SDState::Idle) { _upload_status = UploadStatusType::FAILED; grbl_send(CLIENT_ALL, "[MSG:Upload cancelled]\r\n"); pushError(ESP_ERROR_UPLOAD_CANCELLED, "Upload cancelled"); } else { - set_sd_state(SDCARD_BUSY_UPLOADING); + set_sd_state(SDState::BusyUploading); //delete file on SD Card if already present if (SD.exists(filename)) { SD.remove(filename); @@ -1464,7 +1476,7 @@ namespace WebUI { //************** } else if (upload.status == UPLOAD_FILE_WRITE) { vTaskDelay(1 / portTICK_RATE_MS); - if (sdUploadFile && (_upload_status == UploadStatusType::ONGOING) && (get_sd_state(false) == SDCARD_BUSY_UPLOADING)) { + if (sdUploadFile && (_upload_status == UploadStatusType::ONGOING) && (get_sd_state(false) == SDState::BusyUploading)) { //no error write post data if (upload.currentSize != sdUploadFile.write(upload.buf, upload.currentSize)) { _upload_status = UploadStatusType::FAILED; @@ -1502,7 +1514,7 @@ namespace WebUI { } if (_upload_status == UploadStatusType::ONGOING) { _upload_status = UploadStatusType::SUCCESSFUL; - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); } else { _upload_status = UploadStatusType::FAILED; pushError(ESP_ERROR_UPLOAD, "Upload error"); @@ -1510,7 +1522,7 @@ namespace WebUI { } else { //Upload cancelled _upload_status = UploadStatusType::FAILED; - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); grbl_send(CLIENT_ALL, "[MSG:Upload failed]\r\n"); if (sdUploadFile) { sdUploadFile.close(); @@ -1528,7 +1540,7 @@ namespace WebUI { if (SD.exists(filename)) { SD.remove(filename); } - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); } COMMANDS::wait(0); } diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index 60be717d..6a09ba83 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -164,6 +164,9 @@ namespace WebUI { } Error WebCommand::action(char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { + if (_cmdChecker && _cmdChecker()) { + return Error::AnotherInterfaceBusy; + } char empty = '\0'; if (!value) { value = ∅ @@ -300,11 +303,11 @@ namespace WebUI { } if (!SPIFFS.exists(path)) { webPrintln("Error: No such file!"); - return Error::SdFileNotFound; + return Error::FsFileNotFound; } File currentfile = SPIFFS.open(path, FILE_READ); if (!currentfile) { //if file open success - return Error::SdFailedOpenFile; + return Error::FsFailedOpenFile; } //until no line in file Error err; @@ -336,11 +339,11 @@ namespace WebUI { } if (!SPIFFS.exists(path)) { webPrintln("Error: No such file!"); - return Error::SdFileNotFound; + return Error::FsFileNotFound; } File currentfile = SPIFFS.open(path, FILE_READ); if (!currentfile) { - return Error::SdFailedOpenFile; + return Error::FsFailedOpenFile; } while (currentfile.available()) { // String currentline = currentfile.readStringUntil('\n'); @@ -464,7 +467,7 @@ namespace WebUI { webPrintln("Signal: ", String(wifi_config.getSignal(WiFi.RSSI())) + "%"); uint8_t PhyMode; - esp_wifi_get_protocol(ESP_IF_WIFI_STA, &PhyMode); + esp_wifi_get_protocol(WIFI_IF_STA, &PhyMode); const char* modeName; switch (PhyMode) { case WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N: @@ -499,7 +502,7 @@ namespace WebUI { print_mac("AP", WiFi.softAPmacAddress()); wifi_config_t conf; - esp_wifi_get_config(ESP_IF_WIFI_AP, &conf); + esp_wifi_get_config(WIFI_IF_AP, &conf); webPrintln("SSID: ", (const char*)conf.ap.ssid); webPrintln("Visible: ", (conf.ap.ssid_hidden == 0) ? "Yes" : "No"); @@ -680,20 +683,20 @@ namespace WebUI { if (path[0] != '/') { path = "/" + path; } - int8_t state = get_sd_state(true); - if (state != SDCARD_IDLE) { - if (state == SDCARD_NOT_PRESENT) { + SDState state = get_sd_state(true); + if (state != SDState::Idle) { + if (state == SDState::NotPresent) { webPrintln("No SD Card"); - return Error::SdFailedMount; + return Error::FsFailedMount; } else { webPrintln("SD Card Busy"); - return Error::SdFailedBusy; + return Error::FsFailedBusy; } } if (!openFile(SD, path.c_str())) { - report_status_message(Error::SdFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL); + report_status_message(Error::FsFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL); webPrintln(""); - return Error::SdFailedOpenFile; + return Error::FsFailedOpenFile; } return Error::Ok; } @@ -717,6 +720,10 @@ namespace WebUI { static Error runSDFile(char* parameter, AuthenticationLevel auth_level) { // ESP220 Error err; + if (sys.state == State::Alarm) { + webPrintln("Alarm"); + return Error::IdleError; + } if (sys.state != State::Idle) { webPrintln("Busy"); return Error::IdleError; @@ -746,9 +753,9 @@ namespace WebUI { webPrintln("Missing file name!"); return Error::InvalidValue; } - int8_t state = get_sd_state(true); - if (state != SDCARD_IDLE) { - webPrintln((state == SDCARD_NOT_PRESENT) ? "No SD card" : "Busy"); + SDState state = get_sd_state(true); + if (state != SDState::Idle) { + webPrintln((state == SDState::NotPresent) ? "No SD card" : "Busy"); return Error::Ok; } String path = parameter; @@ -758,18 +765,18 @@ namespace WebUI { File file2del = SD.open(path); if (!file2del) { webPrintln("Cannot stat file!"); - return Error::SdFileNotFound; + return Error::FsFileNotFound; } if (file2del.isDirectory()) { if (!SD.rmdir(path)) { webPrintln("Cannot delete directory! Is directory empty?"); - return Error::SdFailedDelDir; + return Error::FsFailedDelDir; } webPrintln("Directory deleted."); } else { if (!SD.remove(path)) { webPrintln("Cannot delete file!"); - return Error::SdFailedDelFile; + return Error::FsFailedDelFile; } webPrintln("File deleted."); } @@ -778,14 +785,14 @@ namespace WebUI { } static Error listSDFiles(char* parameter, AuthenticationLevel auth_level) { // ESP210 - int8_t state = get_sd_state(true); - if (state != SDCARD_IDLE) { - if (state == SDCARD_NOT_PRESENT) { + SDState state = get_sd_state(true); + if (state != SDState::Idle) { + if (state == SDState::NotPresent) { webPrintln("No SD Card"); - return Error::SdFailedMount; + return Error::FsFailedMount; } else { webPrintln("SD Card Busy"); - return Error::SdFailedBusy; + return Error::FsFailedBusy; } } webPrintln(""); @@ -800,9 +807,35 @@ namespace WebUI { } #endif + void listDirLocalFS(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) { + //char temp_filename[128]; // to help filter by extension TODO: 128 needs a definition based on something + File root = fs.open(dirname); + if (!root) { + //FIXME: need proper error for FS and not usd sd one + report_status_message(Error::FsFailedOpenDir, client); + return; + } + if (!root.isDirectory()) { + //FIXME: need proper error for FS and not usd sd one + report_status_message(Error::FsDirNotFound, client); + return; + } + File file = root.openNextFile(); + while (file) { + if (file.isDirectory()) { + if (levels) { + listDirLocalFS(fs, file.name(), levels - 1, client); + } + } else { + grbl_sendf(CLIENT_ALL, "[FILE:%s|SIZE:%d]\r\n", file.name(), file.size()); + } + file = root.openNextFile(); + } + } + static Error listLocalFiles(char* parameter, AuthenticationLevel auth_level) { // No ESP command webPrintln(""); - listDir(SPIFFS, "/", 10, espresponse->client()); + listDirLocalFS(SPIFFS, "/", 10, espresponse->client()); String ssd = "[Local FS Free:" + ESPResponseStream::formatBytes(SPIFFS.totalBytes() - SPIFFS.usedBytes()); ssd += " Used:" + ESPResponseStream::formatBytes(SPIFFS.usedBytes()); ssd += " Total:" + ESPResponseStream::formatBytes(SPIFFS.totalBytes()); @@ -851,15 +884,17 @@ namespace WebUI { const char* resp = "No SD card"; #ifdef ENABLE_SD_CARD switch (get_sd_state(true)) { - case SDCARD_IDLE: + case SDState::Idle: resp = "SD card detected"; break; - case SDCARD_NOT_PRESENT: + case SDState::NotPresent: resp = "No SD card"; break; default: resp = "Busy"; } +#else + resp = "SD card not enabled"; #endif webPrintln(resp); return Error::Ok; @@ -1017,7 +1052,7 @@ namespace WebUI { // WU - need user or admin password to set // WA - need admin password to set #ifdef WEB_COMMON - new WebCommand(NULL, WEBCMD, WG, "ESP800", "Firmware/Info", showFwInfo); + new WebCommand(NULL, WEBCMD, WG, "ESP800", "Firmware/Info", showFwInfo, anyState); new WebCommand(NULL, WEBCMD, WU, "ESP720", "LocalFS/Size", SPIFFSSize); new WebCommand("FORMAT", WEBCMD, WA, "ESP710", "LocalFS/Format", formatSpiffs); new WebCommand("path", WEBCMD, WU, "ESP701", "LocalFS/Show", showLocalFile); @@ -1035,14 +1070,14 @@ namespace WebUI { #endif #ifdef WEB_COMMON new WebCommand("RESTART", WEBCMD, WA, "ESP444", "System/Control", setSystemMode); - new WebCommand(NULL, WEBCMD, WU, "ESP420", "System/Stats", showSysStats); + new WebCommand(NULL, WEBCMD, WU, "ESP420", "System/Stats", showSysStats, anyState); #endif #ifdef ENABLE_WIFI new WebCommand(NULL, WEBCMD, WU, "ESP410", "WiFi/ListAPs", listAPs); #endif #ifdef WEB_COMMON new WebCommand("P=position T=type V=value", WEBCMD, WA, "ESP401", "WebUI/Set", setWebSetting); - new WebCommand(NULL, WEBCMD, WU, "ESP400", "WebUI/List", listSettings); + new WebCommand(NULL, WEBCMD, WU, "ESP400", "WebUI/List", listSettings, anyState); #endif #ifdef ENABLE_SD_CARD new WebCommand("path", WEBCMD, WU, "ESP221", "SD/Show", showSDFile); @@ -1059,15 +1094,15 @@ namespace WebUI { new WebCommand("IP=ipaddress MSK=netmask GW=gateway", WEBCMD, WA, "ESP103", "Sta/Setup", showSetStaParams); #endif #ifdef WEB_COMMON - new WebCommand(NULL, WEBCMD, WG, "ESP0", "WebUI/Help", showWebHelp); - new WebCommand(NULL, WEBCMD, WG, "ESP", "WebUI/Help", showWebHelp); + new WebCommand(NULL, WEBCMD, WG, "ESP0", "WebUI/Help", showWebHelp, anyState); + new WebCommand(NULL, WEBCMD, WG, "ESP", "WebUI/Help", showWebHelp, anyState); #endif // WebUI Settings // Standard WEBUI authentication is user+ to get, admin to set unless otherwise specified #ifdef ENABLE_NOTIFICATIONS notification_ts = new StringSetting( "Notification Settings", WEBSET, WA, NULL, "Notification/TS", DEFAULT_TOKEN, 0, MAX_NOTIFICATION_SETTING_LENGTH, NULL); - notification_t2 = new StringSetting("Notification Token 2", + notification_t2 = new StringSetting("Notification Token 2", WEBSET, WA, NULL, @@ -1076,7 +1111,7 @@ namespace WebUI { MIN_NOTIFICATION_TOKEN_LENGTH, MAX_NOTIFICATION_TOKEN_LENGTH, NULL); - notification_t1 = new StringSetting("Notification Token 1", + notification_t1 = new StringSetting("Notification Token 1", WEBSET, WA, NULL, @@ -1085,8 +1120,8 @@ namespace WebUI { MIN_NOTIFICATION_TOKEN_LENGTH, MAX_NOTIFICATION_TOKEN_LENGTH, NULL); - notification_type = - new EnumSetting("Notification type", WEBSET, WA, NULL, "Notification/Type", DEFAULT_NOTIFICATION_TYPE, ¬ificationOptions); + notification_type = new EnumSetting( + "Notification type", WEBSET, WA, NULL, "Notification/Type", DEFAULT_NOTIFICATION_TYPE, ¬ificationOptions, NULL); #endif #ifdef ENABLE_AUTHENTICATION user_password = new StringSetting("User password", @@ -1122,16 +1157,16 @@ namespace WebUI { #ifdef WIFI_OR_BLUETOOTH // user+ to get, admin to set - wifi_radio_mode = new EnumSetting("Radio mode", WEBSET, WA, "ESP110", "Radio/Mode", DEFAULT_RADIO_MODE, &radioEnabledOptions); + wifi_radio_mode = new EnumSetting("Radio mode", WEBSET, WA, "ESP110", "Radio/Mode", DEFAULT_RADIO_MODE, &radioEnabledOptions, NULL); #endif #ifdef ENABLE_WIFI telnet_port = new IntSetting( "Telnet Port", WEBSET, WA, "ESP131", "Telnet/Port", DEFAULT_TELNETSERVER_PORT, MIN_TELNET_PORT, MAX_TELNET_PORT, NULL); - telnet_enable = new EnumSetting("Telnet Enable", WEBSET, WA, "ESP130", "Telnet/Enable", DEFAULT_TELNET_STATE, &onoffOptions); + telnet_enable = new EnumSetting("Telnet Enable", WEBSET, WA, "ESP130", "Telnet/Enable", DEFAULT_TELNET_STATE, &onoffOptions, NULL); http_port = new IntSetting("HTTP Port", WEBSET, WA, "ESP121", "Http/Port", DEFAULT_WEBSERVER_PORT, MIN_HTTP_PORT, MAX_HTTP_PORT, NULL); - http_enable = new EnumSetting("HTTP Enable", WEBSET, WA, "ESP120", "Http/Enable", DEFAULT_HTTP_STATE, &onoffOptions); + http_enable = new EnumSetting("HTTP Enable", WEBSET, WA, "ESP120", "Http/Enable", DEFAULT_HTTP_STATE, &onoffOptions, NULL); wifi_hostname = new StringSetting("Hostname", WEBSET, WA, @@ -1159,7 +1194,7 @@ namespace WebUI { wifi_sta_netmask = new IPaddrSetting("Station Static Mask", WEBSET, WA, NULL, "Sta/Netmask", DEFAULT_STA_MK, NULL); wifi_sta_gateway = new IPaddrSetting("Station Static Gateway", WEBSET, WA, NULL, "Sta/Gateway", DEFAULT_STA_GW, NULL); wifi_sta_ip = new IPaddrSetting("Station Static IP", WEBSET, WA, NULL, "Sta/IP", DEFAULT_STA_IP, NULL); - wifi_sta_mode = new EnumSetting("Station IP Mode", WEBSET, WA, "ESP102", "Sta/IPMode", DEFAULT_STA_IP_MODE, &staModeOptions); + wifi_sta_mode = new EnumSetting("Station IP Mode", WEBSET, WA, "ESP102", "Sta/IPMode", DEFAULT_STA_IP_MODE, &staModeOptions, NULL); // no get, admin to set wifi_sta_password = new StringSetting("Station Password", WEBSET, diff --git a/Grbl_Esp32/src/WebUI/WifiConfig.cpp b/Grbl_Esp32/src/WebUI/WifiConfig.cpp index d918e7c7..f3ca256c 100644 --- a/Grbl_Esp32/src/WebUI/WifiConfig.cpp +++ b/Grbl_Esp32/src/WebUI/WifiConfig.cpp @@ -71,7 +71,7 @@ namespace WebUI { } result += "Mode=AP:SSDI="; wifi_config_t conf; - esp_wifi_get_config(ESP_IF_WIFI_AP, &conf); + esp_wifi_get_config(WIFI_IF_AP, &conf); result += (const char*)conf.ap.ssid; result += ":IP="; result += WiFi.softAPIP().toString(); @@ -115,6 +115,9 @@ namespace WebUI { bool WiFiConfig::isHostnameValid(const char* hostname) { //limited size + if (!hostname) { + return true; + } char c; // length is checked automatically by string setting //only letter and digit @@ -139,6 +142,9 @@ namespace WebUI { //char c; // length is checked automatically by string setting //only printable + if (!ssid) { + return true; + } for (int i = 0; i < strlen(ssid); i++) { if (!isPrintable(ssid[i])) { return false; @@ -152,6 +158,9 @@ namespace WebUI { */ bool WiFiConfig::isPasswordValid(const char* password) { + if (!password) { + return true; + } if (strlen(password) == 0) { return true; //open network } diff --git a/README.md b/README.md index 7ee0651d..4a5b33eb 100644 --- a/README.md +++ b/README.md @@ -87,7 +87,7 @@ The Wifi and WebUI is based on [this project.](https://github.com/luc-github/ESP ### Contribute -![](http://www.buildlog.net/blog/wp-content/uploads/2018/07/slack_hash_128.png) There is a slack channel for the development this project. Ask for an Invite + There is a Discord server for the development this project. Ask for an invite ### FAQ diff --git a/embedded/package-lock.json b/embedded/package-lock.json index 156dfe5a..2520c834 100644 --- a/embedded/package-lock.json +++ b/embedded/package-lock.json @@ -1861,7 +1861,8 @@ "ansi-regex": { "version": "2.1.1", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "aproba": { "version": "1.2.0", @@ -1882,12 +1883,14 @@ "balanced-match": { "version": "1.0.0", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "brace-expansion": { "version": "1.1.11", "bundled": true, "dev": true, + "optional": true, "requires": { "balanced-match": "^1.0.0", "concat-map": "0.0.1" @@ -1902,17 +1905,20 @@ "code-point-at": { "version": "1.1.0", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "concat-map": { "version": "0.0.1", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "console-control-strings": { "version": "1.1.0", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "core-util-is": { "version": "1.0.2", @@ -2029,11 +2035,6 @@ "inherits": { "version": "2.0.3", "bundled": true, - "dev": true - }, - "ini": { - "version": "1.3.5", - "bundled": true, "dev": true, "optional": true }, @@ -2041,6 +2042,7 @@ "version": "1.0.0", "bundled": true, "dev": true, + "optional": true, "requires": { "number-is-nan": "^1.0.0" } @@ -2055,6 +2057,7 @@ "version": "3.0.4", "bundled": true, "dev": true, + "optional": true, "requires": { "brace-expansion": "^1.1.7" } @@ -2062,12 +2065,14 @@ "minimist": { "version": "0.0.8", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "minipass": { "version": "2.3.5", "bundled": true, "dev": true, + "optional": true, "requires": { "safe-buffer": "^5.1.2", "yallist": "^3.0.0" @@ -2086,6 +2091,7 @@ "version": "0.5.1", "bundled": true, "dev": true, + "optional": true, "requires": { "minimist": "0.0.8" } @@ -2166,7 +2172,8 @@ "number-is-nan": { "version": "1.0.1", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "object-assign": { "version": "4.1.1", @@ -2178,6 +2185,7 @@ "version": "1.4.0", "bundled": true, "dev": true, + "optional": true, "requires": { "wrappy": "1" } @@ -2263,7 +2271,8 @@ "safe-buffer": { "version": "5.1.2", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "safer-buffer": { "version": "2.1.2", @@ -2299,6 +2308,7 @@ "version": "1.0.2", "bundled": true, "dev": true, + "optional": true, "requires": { "code-point-at": "^1.0.0", "is-fullwidth-code-point": "^1.0.0", @@ -2318,6 +2328,7 @@ "version": "3.0.1", "bundled": true, "dev": true, + "optional": true, "requires": { "ansi-regex": "^2.0.0" } @@ -2361,12 +2372,14 @@ "wrappy": { "version": "1.0.2", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "yallist": { "version": "3.0.3", "bundled": true, - "dev": true + "dev": true, + "optional": true } } }, @@ -3466,9 +3479,9 @@ "dev": true }, "ini": { - "version": "1.3.5", - "resolved": "https://registry.npmjs.org/ini/-/ini-1.3.5.tgz", - "integrity": "sha512-RZY5huIKCMRWDUqZlEi72f/lmXKMvuszcMBduliQ3nnWbx9X/ZBQO7DijMEYS9EhHBb2qacRUMtC7svLwe0lcw==", + "version": "1.3.7", + "resolved": "https://registry.npmjs.org/ini/-/ini-1.3.7.tgz", + "integrity": "sha512-iKpRpXP+CrP2jyrxvg1kMUpXDyRUFDWurxbnVT1vQPx+Wz9uCYsMIqYuSBLV+PAaZG/d7kRLKRFc9oDMsH+mFQ==", "dev": true }, "interpret": { diff --git a/platformio.ini b/platformio.ini index cc2807df..735a5f47 100644 --- a/platformio.ini +++ b/platformio.ini @@ -1,9 +1,9 @@ [platformio] -src_dir=Grbl_Esp32 -lib_dir=libraries -test_dir=Grbl_Esp32/test -data_dir=Grbl_Esp32/data -default_envs=release +src_dir = Grbl_Esp32 +lib_dir = libraries +test_dir = Grbl_Esp32/test +data_dir = Grbl_Esp32/data +default_envs = release ;extra_configs=debug.ini build_dir = Firmware @@ -31,12 +31,11 @@ build_flags = -Wno-unused-variable -Wno-unused-function - [env] lib_deps = TMCStepper@>=0.7.0,<1.0.0 platform = espressif32 -board = esp32dev +board = esp32dev@3.0.0 framework = arduino upload_speed = 921600 board_build.partitions = min_spiffs.csv @@ -46,7 +45,6 @@ monitor_flags = --echo --filter=esp32_exception_decoder board_build.f_cpu = 240000000L -; set frequency to 80MHz board_build.f_flash = 80000000L board_build.flash_mode = qio build_flags = ${common.build_flags} @@ -55,10 +53,16 @@ src_filter = -<.git/> - - - - [env:release] +lib_deps = + TMCStepper@>=0.7.0,<1.0.0 + ESP8266 and ESP32 OLED driver for SSD1306 displays@^4.2.0 [env:debug] build_type = debug +lib_deps = + TMCStepper@>=0.7.0,<1.0.0 + ESP8266 and ESP32 OLED driver for SSD1306 displays@^4.2.0 [env:test] build_type = debug -test_build_project_src = true \ No newline at end of file +test_build_project_src = true