From 5316c64abb4d2de994799c09621c777391cd2386 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 16 Aug 2021 19:02:10 -0500 Subject: [PATCH] Devt (#956) * changing to EXTENDED type from GRBL type to prevent sender issues when running 1585 * Oled2 (#834) * WIP * WIP * Update platformio.ini * WIP * Cleanup * Update platformio.ini * Turn off soft limits with max travel (#836) https://github.com/bdring/Grbl_Esp32/issues/831 * Yalang YL620 VFD (#838) * New SpindleType YL620 Files for new SpindleType Yalang 620. So far the contents are a duplicate of H2ASpindle.h and H2ASpindle.cpp * Added register documentation and implemented read and write data packets * Some fixes, mostly regarding RX packet length * OLED and Other Updates (#844) * publish * Updates - CoreXY and OLED - Moved position calculation out of report_realtime_status(...) so other functions can access it. - Added a function to check if a limit switch is defined - CoreXY fixed bug in forward kinematics when midtbot is used. - Modified OLED display. * Cleanup for PR * Delete midtbot_x2.h * Incorporated PR 846 - Some OLED cleanup - verified correct forward kinematics on MidTbot * Pio down rev (#850) * Update platformio.ini * Update Grbl.h * Use local UART driver not HardwareSerial (#857) * Use local UART driver not HardwareSerial The HardwareSerial driver is broken in Arduino framework versions 1.0.5 and 1.0.6 . https://github.com/espressif/arduino-esp32/issues/5005 Instead of waiting for a fix, I wrote a very simple UART driver that does exactly what we need with no unnecessary bells and whistles to cause problems. * Added missing files, changed method signatures The methods implemented by the UART class now have the same signatures as the HardwareSerial class, so it will be easy to switch back if we need to. * Incorporated suggestions from Stefan * Fixed TX_IDLE_NUM bug reported by mstrens * Quick test for Bf: problem This is not the final solution. * Fixed stupid typo in last commit * Another test - check for client_buffer space * Use the esp-idf uart driver You can revert to the direct driver for testing by defining DIRECT_UART * Uart class now supports VFD and TMC * data bits, stop bits, parity as enum classes The constants for data bits, stop bits, and parity were changed to enum classes so the compiler can check for argument order mismatches. * Set half duplex after uart init * Init TMC UART only once * rx/tx pin order mixup, missing _uart_started * Test: use Arduino Serial This reverts to the Arduino serial driver for UI communication, leaving the VFS comms on the Uart class on top of the esp_idf UART driver. You can switch back and forth with the define REVERT_TO_SERIAL line in Serial.cpp * REVERT_TO_ARDUINO_SERIAL off by default * Added debug messages * Update Grbl.h * Update platformio.ini Co-authored-by: bdring * Fixed spindle sync for all VFD spindles (#868) * Implemented H2A spindle sync fix. Untested. * Changed the spindle sync fix to be in the VFD code. * Update Grbl.h Co-authored-by: Stefan de Bruijn Co-authored-by: bdring * New jog fix (#872) * Applied 741 to new Devt * Make kinematics routines weak to eliminate ifdefs * Fixed warning * Update build date Co-authored-by: bdring * need to override set_rpm * trying set_rpm override * it turns on! * start/stop and set speed all working * cleanup * fixing machine.h * fix .gitignore *&vi .gitignore didn't work anyway * forgot to get rid of hard coded max_freq, fixed now * changed 'speed' to 'freq' * reverting platformio.ini * minor readme update * removed debug msg * Big kinematics cleanup (#875) * Applied 741 to new Devt * Make kinematics routines weak to eliminate ifdefs * Fixed warning * Big kinematics cleanup * Cleanup * no isCancelled arg for jog_execute; return it instead * WIP * Made OLED compliant with new kinematics * Added system_get_mpos * system_get_mpos() returns float* * WIP * Cleanup after testing - Had MPos and WPos text on OLED backwards. - Added my cartesian test def - Will remove test defs before merging to devt. * Cleanup of remaining user optional code. * Fixed delta kinematics loop ending early. * Account for jog cancel in saved motor positions * Update Grbl.h Co-authored-by: bdring * Add the Root 4 Lite CNC machine to the Machines folder (#886) * update for the Root 4 Lite * Return machine.h to use test_drive.hy * Removed some machine definitions Co-authored-by: bdring * YL620_Fix (#941) * YL620_Fix Fix per ... https://github.com/bdring/Grbl_Esp32/issues/926#issuecomment-867885248 Added CNC_xPro machine def * Update Grbl.h * Delete CNC_xPRO_V5_XYYZ_PWM_NO.h * fixes for RPM * note * fixed rx length * fix smell and update readme * clang format * trying to fix newlines * trying to fix newlines part 2 * fix cast issue * fixed spinup spindown swap. Co-authored-by: me Co-authored-by: Mitch Bradley Co-authored-by: marcosprojects Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Pete Co-authored-by: Jesse Schoch --- Grbl_Esp32/src/Grbl.h | 2 +- .../Root_Controller_Root_4_Lite_RS485.h | 136 ++++++++++++ Grbl_Esp32/src/Machines/i2s_out_xxyyzz.h | 102 --------- Grbl_Esp32/src/Machines/i2s_out_xyzabc.h | 105 --------- .../src/Machines/i2s_out_xyzabc_trinamic.h | 106 --------- Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h | 84 ------- Grbl_Esp32/src/SettingsDefinitions.cpp | 1 + Grbl_Esp32/src/Spindles/PWMSpindle.cpp | 4 +- Grbl_Esp32/src/Spindles/Spindle.cpp | 10 +- Grbl_Esp32/src/Spindles/Spindle.h | 1 + Grbl_Esp32/src/Spindles/TecoL510.cpp | 210 ++++++++++++++++++ Grbl_Esp32/src/Spindles/TecoL510.h | 48 ++++ Grbl_Esp32/src/Spindles/TecoL510_README.md | 31 +++ 13 files changed, 438 insertions(+), 402 deletions(-) create mode 100644 Grbl_Esp32/src/Machines/Root_Controller_Root_4_Lite_RS485.h delete mode 100644 Grbl_Esp32/src/Machines/i2s_out_xxyyzz.h delete mode 100644 Grbl_Esp32/src/Machines/i2s_out_xyzabc.h delete mode 100644 Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h delete mode 100644 Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h create mode 100644 Grbl_Esp32/src/Spindles/TecoL510.cpp create mode 100644 Grbl_Esp32/src/Spindles/TecoL510.h create mode 100644 Grbl_Esp32/src/Spindles/TecoL510_README.md diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index f0dbd60d..b89b7c4a 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210424"; +const char* const GRBL_VERSION_BUILD = "20210816"; //#include #include 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..d13bf586 --- /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_NUM_22 +#define I2S_OUT_WS GPIO_NUM_21 +#define I2S_OUT_DATA GPIO_NUM_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_NUM_2 +#define Y_LIMIT_PIN GPIO_NUM_26 +#define Y2_LIMIT_PIN GPIO_NUM_27 +#define Z_LIMIT_PIN GPIO_NUM_14 +#define PROBE_PIN GPIO_NUM_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 "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_NUM_22 -#define I2S_OUT_WS GPIO_NUM_17 -#define I2S_OUT_DATA GPIO_NUM_21 - - -#define STEPPER_MS1 GPIO_NUM_23 // MOSI -#define STEPPER_MS2 GPIO_NUM_18 // SCK - -#define STEPPER_X_MS3 I2SO(3) // X_CS -#define STEPPER_Y_MS3 I2SO(6) // Y_CS -#define STEPPER_Z_MS3 I2SO(11) // Z_CS -#define STEPPER_A_MS3 I2SO(14) // A_CS -#define STEPPER_B_MS3 I2SO(19) // B_CS -#define STEPPER_C_MS3 I2SO(22) // C_CS - -#define STEPPER_RESET GPIO_NUM_19 - -#define X_DISABLE_PIN I2SO(0) -#define X_DIRECTION_PIN I2SO(1) -#define X_STEP_PIN I2SO(2) - -#define 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_NUM_26 -#define SPINDLE_ENABLE_PIN GPIO_NUM_4 -#define SPINDLE_DIR_PIN GPIO_NUM_16 - -#define X_LIMIT_PIN GPIO_NUM_36 -#define Y_LIMIT_PIN GPIO_NUM_39 -#define Z_LIMIT_PIN GPIO_NUM_34 -//#define A_LIMIT_PIN GPIO_NUM_35 -//#define B_LIMIT_PIN GPIO_NUM_32 -//#define C_LIMIT_PIN GPIO_NUM_33 - -#define PROBE_PIN GPIO_NUM_25 - -#define COOLANT_MIST_PIN GPIO_NUM_2 - -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/src/Machines/i2s_out_xyzabc.h b/Grbl_Esp32/src/Machines/i2s_out_xyzabc.h deleted file mode 100644 index 37872267..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_NUM_22 -#define I2S_OUT_WS GPIO_NUM_17 -#define I2S_OUT_DATA GPIO_NUM_21 - - -#define STEPPER_MS1 GPIO_NUM_23 // MOSI -#define STEPPER_MS2 GPIO_NUM_18 // SCK - -#define STEPPER_X_MS3 I2SO(3) // X_CS -#define STEPPER_Y_MS3 I2SO(6) // Y_CS -#define STEPPER_Z_MS3 I2SO(11) // Z_CS -#define STEPPER_A_MS3 I2SO(14) // A_CS -#define STEPPER_B_MS3 I2SO(19) // B_CS -#define STEPPER_C_MS3 I2SO(22) // C_CS - -#define STEPPER_RESET GPIO_NUM_19 - -#define X_DISABLE_PIN I2SO(0) -#define X_DIRECTION_PIN I2SO(1) -#define X_STEP_PIN I2SO(2) - -#define Y_DIRECTION_PIN I2SO(4) -#define Y_STEP_PIN I2SO(5) -#define Y_DISABLE_PIN I2SO(7) - -#define Z_DISABLE_PIN I2SO(8) -#define Z_DIRECTION_PIN I2SO(9) -#define Z_STEP_PIN I2SO(10) - -#define A_DIRECTION_PIN I2SO(12) -#define A_STEP_PIN I2SO(13) -#define A_DISABLE_PIN I2SO(15) - -#define B_DISABLE_PIN I2SO(16) -#define B_DIRECTION_PIN I2SO(17) -#define B_STEP_PIN I2SO(18) -//#define B_CS_PIN I2SO(19) - -#define C_DIRECTION_PIN I2SO(20) -#define C_STEP_PIN I2SO(21) -//#define C_CS_PIN I2SO(22) -#define C_DISABLE_PIN I2SO(23) - - -#define SPINDLE_TYPE SpindleType::PWM // only one spindle at a time -#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 -#define SPINDLE_ENABLE_PIN GPIO_NUM_4 -#define SPINDLE_DIR_PIN GPIO_NUM_16 - -#define X_LIMIT_PIN GPIO_NUM_36 -#define Y_LIMIT_PIN GPIO_NUM_39 -#define Z_LIMIT_PIN GPIO_NUM_34 -#define A_LIMIT_PIN GPIO_NUM_35 -#define B_LIMIT_PIN GPIO_NUM_32 -#define C_LIMIT_PIN GPIO_NUM_33 - -#define PROBE_PIN GPIO_NUM_25 - -#define COOLANT_MIST_PIN GPIO_NUM_2 - - - -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h b/Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h deleted file mode 100644 index 3d5cffb7..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_NUM_22 -#define I2S_OUT_WS GPIO_NUM_17 -#define I2S_OUT_DATA GPIO_NUM_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_NUM_26 -#define SPINDLE_ENABLE_PIN GPIO_NUM_4 -#define SPINDLE_DIR_PIN GPIO_NUM_16 -*/ -#define X_LIMIT_PIN GPIO_NUM_33 -#define Y_LIMIT_PIN GPIO_NUM_32 -#define Z_LIMIT_PIN GPIO_NUM_35 -#define A_LIMIT_PIN GPIO_NUM_34 -#define B_LIMIT_PIN GPIO_NUM_39 -#define C_LIMIT_PIN GPIO_NUM_36 - -#define SPINDLE_TYPE SpindleType::RELAY -#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 - -#define PROBE_PIN GPIO_NUM_25 - -#define COOLANT_MIST_PIN GPIO_NUM_2 - -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h deleted file mode 100644 index fc9b45f6..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_NUM_12 -#define X_DIRECTION_PIN GPIO_NUM_14 -#define X_CS_PIN GPIO_NUM_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_NUM_27 -#define Y_DIRECTION_PIN GPIO_NUM_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_NUM_15 -#define Z_DIRECTION_PIN GPIO_NUM_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_NUM_21 - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_25 -#define SPINDLE_ENABLE_PIN GPIO_NUM_4 - -// Relay operation -// Install Jumper near relay -// For spindle Use max RPM of 1 -// For PWM remove jumper and set MAX RPM to something higher ($30 setting) -// Interlock jumper along top edge needs to be installed for both versions -#define DEFAULT_SPINDLE_RPM_MAX 1 // Should be 1 for relay operation - -#define PROBE_PIN GPIO_NUM_22 - -#define X_LIMIT_PIN GPIO_NUM_36 -#define Y_LIMIT_PIN GPIO_NUM_39 -#define Z_LIMIT_PIN GPIO_NUM_34 - diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index b98da4e3..e400e129 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -73,6 +73,7 @@ enum_opt_t spindleTypes = { { "BESC", int8_t(SpindleType::BESC) }, { "10V", int8_t(SpindleType::_10V) }, { "H2A", int8_t(SpindleType::H2A) }, + { "YL620", int8_t(SpindleType::YL620) }, // clang-format on }; diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp index 402af6e4..97fcb8c3 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp @@ -166,14 +166,14 @@ namespace Spindles { sys.spindle_speed = 0; stop(); if (use_delays && (_current_state != state)) { - delay(_spinup_delay); + delay(_spindown_delay); } } else { set_dir_pin(state == SpindleState::Cw); set_rpm(rpm); set_enable_pin(state != SpindleState::Disable); // must be done after setting rpm for enable features to work if (use_delays && (_current_state != state)) { - delay(_spindown_delay); + delay(_spinup_delay); } } diff --git a/Grbl_Esp32/src/Spindles/Spindle.cpp b/Grbl_Esp32/src/Spindles/Spindle.cpp index 7dfd8f6b..bc15ab96 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.cpp +++ b/Grbl_Esp32/src/Spindles/Spindle.cpp @@ -39,10 +39,12 @@ #include "BESCSpindle.h" #include "10vSpindle.h" #include "YL620Spindle.h" +#include "TecoL510.h" namespace Spindles { // An instance of each type of spindle is created here. // This allows the spindle to be dynamicly switched + Null null; PWM pwm; Relay relay; @@ -52,7 +54,8 @@ namespace Spindles { H2A h2a; BESC besc; _10v _10v; - YL620 yl620; + YL620 yl620; + L510 l510; void Spindle::select() { switch (static_cast(spindle_type->get())) { @@ -83,6 +86,9 @@ namespace Spindles { case SpindleType::YL620: spindle = &yl620; break; + case SpindleType::L510: + spindle = &l510; + break; case SpindleType::NONE: default: spindle = &null; @@ -109,4 +115,4 @@ namespace Spindles { 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 200e99cd..ff2ea5d4 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.h +++ b/Grbl_Esp32/src/Spindles/Spindle.h @@ -39,6 +39,7 @@ enum class SpindleType : int8_t { _10V, H2A, YL620, + L510 }; #include "../Grbl.h" diff --git a/Grbl_Esp32/src/Spindles/TecoL510.cpp b/Grbl_Esp32/src/Spindles/TecoL510.cpp new file mode 100644 index 00000000..aa6c3d36 --- /dev/null +++ b/Grbl_Esp32/src/Spindles/TecoL510.cpp @@ -0,0 +1,210 @@ +#include "TecoL510.h" + +/* + TecoL510.cpp + + Part of Grbl_ESP32 + 2021 - Jesse Schoch + + 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. + + See TecoL510.md for details +*/ + +namespace Spindles { + L510::L510() : VFD() { + _baudrate = 9600; + _parity = Uart::Parity::None; + // TODO: should defaults be set here? What happens if the motor settings in the VFD are wrong or default? + // I think they are overloaded with DEFAULT_SPINDLE_RPM_MAX and DEFAULT_SPINDLE_RPM_MIN + _max_rpm = 24000; + _min_rpm = 6000; + _max_freq = 40000; + } + + void L510::direction_command(SpindleState mode, ModbusCommand& data) { + // Note: The direction command is always called on M3,M4, and M5 + // This is where the spindle start/stop should be sent + + + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 6; + + data.msg[1] = 0x06; // WRITE + data.msg[2] = 0x25; // Command ID 0x2501 + data.msg[3] = 0x01; + data.msg[4] = 0x00; + switch (mode) { + case SpindleState::Disable: + //data.msg[4] = 0x00; + data.msg[5] = 0x00; + break; + case SpindleState::Cw: + data.msg[5] = 0x01; + break; + case SpindleState::Ccw: + data.msg[5] = 0x03; + break; + } + } + + void L510::set_speed_command(uint32_t rpm, ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 6; + + uint16_t freq = rpm_to_frequency(rpm); + + data.msg[1] = 0x06; // WRITE + data.msg[2] = 0x25; // Command ID 0x2502 + data.msg[3] = 0x02; + data.msg[4] = uint8_t(freq >> 8); // RPM + data.msg[5] = uint8_t(freq & 0xFF); + +#ifdef VFD_DEBUG_MODE2 + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "setting speed to: %d", speed); +#endif + } + uint16_t L510::rpm_to_frequency(uint32_t rpm) { + auto max_rpm = this->_max_rpm; + auto max_freq = this->_max_freq; + uint16_t freq = (uint32_t(rpm) * max_freq) / uint32_t(max_rpm); + if (freq < 0) { + freq = 0; + } + if (freq > max_freq) { + freq = max_freq; + } + return freq; + } + + uint32_t L510::freq_to_rpm(uint16_t freq) { + auto max_rpm = this->_max_rpm; + auto max_freq = this->_max_freq; + uint32_t rpm = (freq * max_rpm) / max_freq; + if (rpm < 0) { + // sometimes it returns -1 which causes an alarm + rpm = 0; + } + return rpm; + } + + VFD::response_parser L510::initialization_sequence(int index, ModbusCommand& data) { + if (index == -1) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 11; + + // read parameters 02-03..02-06 + + // Send: + data.msg[1] = 0x03; // READ + data.msg[2] = 0x02; // 0x0203 = Get max rpm + data.msg[3] = 0x03; + data.msg[4] = 0x00; // Read 4 values + data.msg[5] = 0x04; + + // Recv: ?? + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint32_t rpm = (response[3] << 8) | response[4]; + + // NOTE: the frequency is stored xxx.x but the input command is frequency * 100; + uint16_t freq = ((uint16_t)response[9] << 8) | (uint16_t)response[10]; + freq = freq * 10; + auto l510 = static_cast(vfd); + + l510->_max_rpm = rpm; + l510->_max_freq = freq; + + grbl_msg_sendf( + CLIENT_SERIAL, MsgLevel::Info, "L510 initialized: spindle max_rpm %d max_freq %d", vfd->_max_rpm, l510->_max_freq); + + return true; + }; + } else { + return nullptr; + } + } + VFD::response_parser L510::get_status_ok(ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 5; + + // Send: + data.msg[1] = 0x03; // READ + data.msg[2] = 0x25; // 0x2520 = Get state + data.msg[3] = 0x20; + data.msg[4] = 0x00; // Read 1 value + data.msg[5] = 0x01; + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t vfd_state = ((uint16_t)response[3] << 8) | (uint16_t)response[4]; + + if (bitRead(vfd_state, 3)) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "L510 Fault detected"); + return false; + } + return true; + }; + } + + VFD::response_parser L510::get_current_rpm(ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 5; + + // Send: 01 03 700C 0002 + data.msg[1] = 0x03; // READ + data.msg[2] = 0x25; // 0x2524 = Get output frequency + data.msg[3] = 0x24; + data.msg[4] = 0x00; // Read 1 value + data.msg[5] = 0x01; + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t freq = ((uint16_t)response[3] << 8) | (uint16_t)response[4]; + + auto l510 = static_cast(vfd); + + vfd->_sync_rpm = l510->freq_to_rpm(freq); + return true; + }; + } + + VFD::response_parser L510::get_current_direction(ModbusCommand& data) { + // does this run ever?? + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 5; + + // Send: 01 03 30 00 00 01 + data.msg[1] = 0x03; // READ + data.msg[2] = 0x25; // 0x2520 + data.msg[3] = 0x20; + data.msg[4] = 0x00; // Message ID + data.msg[5] = 0x01; + + // Receive: 01 03 00 02 00 02 + // ----- status + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t got = (uint16_t(response[3]) << 8) | uint16_t(response[4]); + bool dir = bitRead(got, 1); + return true; + }; + } +} diff --git a/Grbl_Esp32/src/Spindles/TecoL510.h b/Grbl_Esp32/src/Spindles/TecoL510.h new file mode 100644 index 00000000..f89de262 --- /dev/null +++ b/Grbl_Esp32/src/Spindles/TecoL510.h @@ -0,0 +1,48 @@ +#pragma once + +#include "VFDSpindle.h" + +/* + TecoL510.h + + Part of Grbl_ESP32 + 2021 - Jesse Schoch + + 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 L510 : public VFD { + protected: + 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; + + // what is this, what should it do? + bool supports_actual_rpm() const override { return true; } + bool safety_polling() const override { return true; } + response_parser get_status_ok(ModbusCommand& data) override; + uint16_t rpm_to_frequency(uint32_t rpm); + uint32_t freq_to_rpm(uint16_t); + //uint32_t set_rpm(uint32_t rpm) override; + void start_spindle(); + + public: + L510(); + uint16_t _max_freq; + }; +} diff --git a/Grbl_Esp32/src/Spindles/TecoL510_README.md b/Grbl_Esp32/src/Spindles/TecoL510_README.md new file mode 100644 index 00000000..9cdfd6e9 --- /dev/null +++ b/Grbl_Esp32/src/Spindles/TecoL510_README.md @@ -0,0 +1,31 @@ +# VFD setup + +Change these values: +1) 00-02, Main Run Command Source Selection, Default = 0 Change to 2 +2) 00-05, Main Freq Command Source Selection, Default = 0 Change to 5 + +Check that these are set to the default factory values: +3) 00-07, Main and Alternative Freq Cmnd Source Selection, Default = 0 +4) 09-00, Assigned Communication Number, Default = 1 this is defined as VFD_RS485_ADDR +5) 09-01, RTU/ASCII Code Selection, Default = 0 Should be 0 (We are using hex "RTU" commands, not ASCII commands) +6) 09-02, Baud Rate Setting, Default = 1 Should be 1 (9600 baud) but can be changed in the TecoL510.cpp +7) 09-03, Stop Bit Selection, Default = 0 Should be 0 (1 stop bit) +8) 09-04, Parity Selection, Default = 0 Should be 0 (Without parity) +9) 09-05, Data Format Selection, Default = 0 Should be 0 (8 data bits) + +# Implementation details + +this was tested with pin 16 RX and 26 TX mapped in the machine config file. Pin 4 was used for RTS but it is not used on the rs485 module I tested with. + +``` +#define VFD_RS485_TXD_PIN GPIO_NUM_26 +#define VFD_RS485_RTS_PIN GPIO_NUM_4 +#define VFD_RS485_RXD_PIN GPIO_NUM_16 +// Not sure why this isn't a setting + +#define DEFAULT_SPINDLE_RPM_MAX 24000.0 // rpm +#define DEFAULT_SPINDLE_RPM_MIN 6000.0 // rpm +``` + +not sure how to detect and parse error messages +max frequency is currenly hardcode