From e51ae8c21eed5d89635c3674d8634b4b63e7ab11 Mon Sep 17 00:00:00 2001 From: bdring Date: Thu, 25 Mar 2021 17:03:21 -0500 Subject: [PATCH] Devt (#842) * 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 Co-authored-by: Mitch Bradley Co-authored-by: marcosprojects --- Grbl_Esp32/Custom/oled_basic.cpp | 288 +++++++++++++++++++++++ Grbl_Esp32/src/CustomCode.cpp | 4 + Grbl_Esp32/src/Grbl.cpp | 2 + Grbl_Esp32/src/Grbl.h | 8 +- Grbl_Esp32/src/Limits.cpp | 3 +- Grbl_Esp32/src/SettingsDefinitions.cpp | 2 +- Grbl_Esp32/src/Spindles/Spindle.cpp | 5 + Grbl_Esp32/src/Spindles/Spindle.h | 1 + Grbl_Esp32/src/Spindles/YL620Spindle.cpp | 241 +++++++++++++++++++ Grbl_Esp32/src/Spindles/YL620Spindle.h | 43 ++++ platformio.ini | 45 ++-- 11 files changed, 621 insertions(+), 21 deletions(-) create mode 100644 Grbl_Esp32/Custom/oled_basic.cpp create mode 100644 Grbl_Esp32/src/Spindles/YL620Spindle.cpp create mode 100644 Grbl_Esp32/src/Spindles/YL620Spindle.h diff --git a/Grbl_Esp32/Custom/oled_basic.cpp b/Grbl_Esp32/Custom/oled_basic.cpp new file mode 100644 index 00000000..49984ab8 --- /dev/null +++ b/Grbl_Esp32/Custom/oled_basic.cpp @@ -0,0 +1,288 @@ +/* + 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" // legacy: #include "SSD1306.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; + +// returns the position of a machine axis +// wpos =true for corrected work postion +float getPosition(uint8_t axis, bool wpos = true) { + float wco; // work coordinate system offset + + float current_position = sys_position[axis] / axis_settings[axis]->steps_per_mm->get(); + + if (wpos) { + // Apply work coordinate offsets and tool length offset to current position. + wco = gc_state.coord_system[axis] + gc_state.coord_offset[axis]; + if (axis == TOOL_LENGTH_OFFSET_AXIS) { + wco += gc_state.tool_length_offset; + } + + current_position -= wco; + } + return current_position; +} + +String getStateText() { + String str = ""; + + switch (sys.state) { + case State::Idle: + str = "Idle"; + break; + case State::Cycle: + str = "Run"; + break; + case State::Hold: + if (!(sys.suspend.bit.jogCancel)) { + str = "Hold:"; + sys.suspend.bit.holdComplete ? str += "0" : str += "1"; // Ready to resume + break; + } // Continues to print jog state during jog cancel. + case State::Jog: + str = "Jog"; + break; + case State::Homing: + str = "Homing"; + break; + case State::Alarm: + str = "Alarm"; + break; + case State::CheckMode: + str = "Check"; + break; + case State::SafetyDoor: + str = "Door:"; + if (sys.suspend.bit.initiateRestore) { + str += "3"; // Restoring + } else { + if (sys.suspend.bit.retractComplete) { + sys.suspend.bit.safetyDoorAjar ? str += "1" : str += "0"; // Door ajar + // Door closed and ready to resume + } else { + str += "2"; // Retracting + } + } + break; + case State::Sleep: + str = "Sleep"; + break; + } + + return str; +} + +// This displays the status of the ESP32 Radios...BT, WiFi, etc +void displayRadioInfo() { + String radio_info = ""; + + const uint8_t row1 = 18; + const uint8_t row2 = 30; + const uint8_t row3 = 42; + + display.setTextAlignment(TEXT_ALIGN_LEFT); + display.setFont(ArialMT_Plain_10); + +#ifdef ENABLE_BLUETOOTH + if (WebUI::wifi_radio_mode->get() == ESP_BT) { + radio_info = String("Bluetooth: ") + WebUI::bt_name->get(); + display.drawString(0, row1, radio_info); + radio_info = String("Status: ") + String(WebUI::SerialBT.hasClient() ? "Connected" : "Not connected"); + display.drawString(0, row2, radio_info); + } +#endif +#ifdef ENABLE_WIFI + if ((WiFi.getMode() == WIFI_MODE_STA) || (WiFi.getMode() == WIFI_MODE_APSTA)) { + radio_info = "STA SSID: " + WiFi.SSID(); + display.drawString(0, row1, radio_info); + + radio_info = "IP: " + WiFi.localIP().toString(); + display.drawString(0, row2, radio_info); + + radio_info = "Status: "; + (WiFi.status() == WL_CONNECTED) ? radio_info += "Connected" : radio_info += "Not connected"; + display.drawString(0, row3, radio_info); + //} + } else if ((WiFi.getMode() == WIFI_MODE_AP) || (WiFi.getMode() == WIFI_MODE_APSTA)) { + radio_info = String("AP SSID: ") + WebUI::wifi_ap_ssid->get(); + + display.drawString(0, row1, radio_info); + + radio_info = "IP: " + WiFi.softAPIP().toString(); + display.drawString(0, row2, radio_info); + } +#endif + +#ifdef WIFI_OR_BLUETOOTH + if (WebUI::wifi_radio_mode->get() == ESP_RADIO_OFF) { + display.drawString(0, row1, "Radio Mode: None"); + } +#else + display.drawString(0, row1, "Wifi and Bluetooth Disabled"); +#endif +} + +void displayDRO() { + char axisVal[20]; + + display.setFont(ArialMT_Plain_16); + display.setTextAlignment(TEXT_ALIGN_LEFT); + display.drawString(0, 13, String('X') + ":"); + display.drawString(0, 30, "Y:"); + display.drawString(0, 47, "Z:"); + + display.setTextAlignment(TEXT_ALIGN_RIGHT); + snprintf(axisVal, 20 - 1, "%.3f", getPosition(X_AXIS, true)); + display.drawString(100, 13, axisVal); + + snprintf(axisVal, 20 - 1, "%.3f", getPosition(Y_AXIS, true)); + display.drawString(100, 30, axisVal); + + snprintf(axisVal, 20 - 1, "%.3f", getPosition(Z_AXIS, true)); + display.drawString(100, 47, axisVal); +} + +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 = getStateText(); + + state_string.toUpperCase(); + + display.setTextAlignment(TEXT_ALIGN_LEFT); + display.setFont(ArialMT_Plain_16); + display.drawString(0, 0, state_string); + + if (get_sd_state(false) == SDState::BusyPrinting) { + display.clear(); + display.setTextAlignment(TEXT_ALIGN_LEFT); + display.setFont(ArialMT_Plain_16); + state_string = "SD File"; + for (int i = 0; i < sd_file_ticker % 10; i++) { + state_string += "."; + } + sd_file_ticker++; + display.drawString(25, 0, state_string); + + 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_16); + display.setTextAlignment(TEXT_ALIGN_CENTER); + display.drawString(64, 25, String(progress) + "%"); + + } else if (sys.state == State::Alarm) { + displayRadioInfo(); + } else { + displayDRO(); + } + 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_16); + + 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/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/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index be3d311d..b3050f92 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -31,6 +31,7 @@ void grbl_init() { WiFi.enableAP(false); WiFi.mode(WIFI_OFF); serial_init(); // Setup serial baud rate and interrupts + 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 // show the map name at startup @@ -116,6 +117,7 @@ void run_once() { void __attribute__((weak)) machine_init() {} +void __attribute__((weak)) display_init() {} /* 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 e17bbfff..bfa875e3 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 = "20210311"; +const char* const GRBL_VERSION_BUILD = "20210320"; //#include #include @@ -65,6 +65,8 @@ const char* const GRBL_VERSION_BUILD = "20210311"; #include "UserOutput.h" +#include + // Do not guard this because it is needed for local files too #include "SDCard.h" @@ -90,8 +92,8 @@ const char* const GRBL_VERSION_BUILD = "20210311"; void grbl_init(); void run_once(); -// Called if USE_MACHINE_INIT is defined -void machine_init(); +void machine_init(); // weak definition in Grbl.cpp +void display_init(); // weak definition in Grbl.cpp bool user_defined_homing(uint8_t cycle_mask); // weak definition in Limits.cpp diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 41596e9d..b87840c1 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -392,13 +392,14 @@ float limitsMinPosition(uint8_t axis) { // Checks and reports if target array exceeds machine travel limits. // Return true if exceeding limits +// Set $/MaxTravel=0 to selectively remove an axis from soft limit checks bool limitsCheckTravel(float* target) { uint8_t idx; auto n_axis = number_axis->get(); 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; } } diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index cb83afcc..b98da4e3 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -301,7 +301,7 @@ void make_settings() { } 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; } diff --git a/Grbl_Esp32/src/Spindles/Spindle.cpp b/Grbl_Esp32/src/Spindles/Spindle.cpp index b9cfed56..7dfd8f6b 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.cpp +++ b/Grbl_Esp32/src/Spindles/Spindle.cpp @@ -38,6 +38,7 @@ #include "H2ASpindle.h" #include "BESCSpindle.h" #include "10vSpindle.h" +#include "YL620Spindle.h" namespace Spindles { // An instance of each type of spindle is created here. @@ -51,6 +52,7 @@ namespace Spindles { H2A h2a; BESC besc; _10v _10v; + YL620 yl620; void Spindle::select() { switch (static_cast(spindle_type->get())) { @@ -78,6 +80,9 @@ namespace Spindles { case SpindleType::H2A: spindle = &h2a; break; + case SpindleType::YL620: + spindle = &yl620; + break; case SpindleType::NONE: default: spindle = &null; diff --git a/Grbl_Esp32/src/Spindles/Spindle.h b/Grbl_Esp32/src/Spindles/Spindle.h index 2f507767..200e99cd 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" diff --git a/Grbl_Esp32/src/Spindles/YL620Spindle.cpp b/Grbl_Esp32/src/Spindles/YL620Spindle.cpp new file mode 100644 index 00000000..9387f563 --- /dev/null +++ b/Grbl_Esp32/src/Spindles/YL620Spindle.cpp @@ -0,0 +1,241 @@ +#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 +*/ + +#include + +namespace Spindles { + void YL620::default_modbus_settings(uart_config_t& uart) { + // sets the uart to 9600 8N1 + VFD::default_modbus_settings(uart); + + uart.baud_rate = 9600; + uart.data_bits = UART_DATA_8_BITS; + uart.parity = UART_PARITY_DISABLE; + uart.stop_bits = UART_STOP_BITS_1; + } + + 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..4e32046b --- /dev/null +++ b/Grbl_Esp32/src/Spindles/YL620Spindle.h @@ -0,0 +1,43 @@ +#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 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 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; } + }; +} diff --git a/platformio.ini b/platformio.ini index cdd2e8a0..7709547c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -1,12 +1,22 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + [platformio] -src_dir=Grbl_Esp32 -lib_dir=libraries -data_dir=Grbl_Esp32/data -default_envs=release +src_dir = Grbl_Esp32 +lib_dir = libraries +data_dir = Grbl_Esp32/data +default_envs = release ;extra_configs=debug.ini [common_env_data] -lib_deps_builtin = +lib_deps_builtin = ArduinoOTA BluetoothSerial DNSServer @@ -23,37 +33,40 @@ lib_deps_builtin = WiFiClientSecure [common] -build_flags = - ;-DMACHINE_FILENAME=test_drive.h ;Remove ";" from the beginning of this line and specify the machine file +build_flags = + ;-DMACHINE_FILENAME=test_drive.h ;Remove ";" from the beginning of this line and specify the machine file -DCORE_DEBUG_LEVEL=0 -Wno-unused-variable -Wno-unused-function - ;-DDEBUG_REPORT_HEAP_SIZE - ;-DDEBUG_REPORT_STACK_FREE [env] -lib_deps = - TMCStepper@>=0.7.0,<1.0.0 +;lib_deps = +; TMCStepper@>=0.7.0,<1.0.0 platform = espressif32 board = esp32dev framework = arduino upload_speed = 921600 board_build.partitions = min_spiffs.csv monitor_speed = 115200 -monitor_flags = +monitor_flags = --eol=CRLF --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} -src_filter = - +<*.h> +<*.s> +<*.S> +<*.cpp> +<*.c> +<*.ino> + - -<.git/> - - - +src_filter = + +<*.h> +<*.s> +<*.S> +<*.cpp> +<*.c> +<*.ino> + + -<.git/> - - - [env:release] +lib_deps = + TMCStepper@>=0.7.0,<1.0.0 + squix78/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 + squix78/ESP8266 and ESP32 OLED driver for SSD1306 displays@^4.2.0