From be08690827387aba086100a0cfedf3f763cff446 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 5 Mar 2021 16:00:19 -0600 Subject: [PATCH] Devt (#795) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fixed various small bugs (#605) * Fixed various small bugs * Fixed potential cast bug * Fixed double reporting of errors Co-authored-by: Stefan de Bruijn * Stallguard tuning (#607) * Devt (#571) * Handles Tranimic drivers errors better - If an unsupported driver is specified, it will give a message and not crash. * Cleaned up unused files Got rid of old unipolar files Got rid of servo axis feature - it is a motor class now Got rid of solenoid pen feature - never really used and it should be a motor class if it is. * Fix ENABLE_AUTHENTICATION (#569) * Fixed authentication code. * Removed another const cast Co-authored-by: Stefan de Bruijn * Fix step leakage with inverted steps (#570) * Fix step leakage with inverted steps * Update build date for merge Co-authored-by: Bart Dring Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Mitch Bradley Co-authored-by: Bart Dring * Update platformio.ini Per PR 583 * Created an enum for mode * Removing some unused machine defs * Added test machine definition * Clean up for PR * Remove test machine def. Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Mitch Bradley Co-authored-by: Bart Dring * Basic testing Complete * Made state variable volatile. * Homing cycle settings (#613) * Initial Tests Complete * Update Grbl.h * Update variables Co-authored-by: Mitch Bradley * fixed dual switches when inverted (#614) * fixed dual switches when inverted * Removed debug message * Cleaning up the machine defs Removed unused #defines. * Store coordinate offsets in NVS (#611) * Store coordinate offsets in NVS * Handle both old Eeprom formats * Implementing fixes (#616) - Stop creating additional tasks when limit_init() gets called again from homing and resets - Explicitly delete an object that was causing a memory loss. * Update Grbl.h * Tweak memory fix and add $H check for $Homing/Cycles * Fix G28.1 and G30.1 * Update Grbl.h * Homing cycle defaults (#624) * Changed to add homing cycle defaults There needs to be a way to set the homing cycle defaults in a machine definition. There will likely be a better way to do this in the future. * Update 10vSpindle.cpp Had wrong error message * Fixed typos and removed obsolete #defines * Probe cleanup (#625) * Cleanup probing code * Update Grbl.h * Update after review * Update error_codes_en_US.csv * More sd_close() to free memory (#622) * Changed buffer sizes to 256 throughout various parts of the program. (#626) This is a patch necessary for F360 personal users, because they decided to add a very lengthy comment... Co-authored-by: Stefan de Bruijn * $sd/show and handle settings in SD files (#629) * $sd/show and handle settings in SD files * Added $LocalFs/Show and fixed $LocalFs/Run output * Infer / at beginning of SD path name The LocalFS path processing code already inserts a / at the beginning of the path is one isn't present. This patch does the same for SD files. * Show $ command responses in WebUI console * Added $Settings/ListChanged AKA $SC This is useful for saving settings in a compact form that leaves defaults unchanged. * $sd/show works in idle or alarm state * Apply idle/alarm checks to SPIFFS files too * Changed sd_close to SD.end() sd_close was a temporary function to check for memory usage * Big BUILD_INFO fix (#632) -- Changes that affect behavior Fixed the bugs with report_build_info() Build info is no longer stored in the fixed "EEPROM" section; instead it is a proper Setting named $Firmware/Build . You can change it in the usual way with $Firmware/Build= $I without the = still works. -- Changes that affect configurability for developers Converted a couple more #defines into enums - SETTINGS_RESTORE_* and BITFLAG_RT_STATUS_* . A side effect of this is that it is no longer possible to configure the behavior of $RST=* by defining SETTINGS_RESTORE_ALL to include only a subset. I think it is a bad idea from a customer support perspective to have the meaning of this command be different for different builds. Changed some of the #define ENABLE_ names to eliminate "EEPROM" -- Changes that are purely cosmetic Clarified descriptions in Config.h, to eliminate spurious/incorrect mentions of "EEPROM" Eliminated all mentions of the name "EEPROM" except the ones that truly mean the EEPROM section, as opposed to generalized non-volatile storage. The contents of SettingsStorage.h and SettingsStorage.cpp, which were really related to coordinate storage in Eeprom, not proper settings, were moved to Eeprom.h and Eeprom.cpp. The SettingsStorage files are gone. Got rid of get_step_pin_mask() and get_direction_pin_mask() because they were just aliases for bit(). That eliminated some junk from the SettingsStorage/Eeprom files. Those files now tightly contain only the residual stuff related to the storage of coordinate data in EEPROM. * Most #defines are gone (#595) * Many more #defines bite the dust * Fixed botch in rt accessory logic * Update Probe.cpp * Update System.cpp * Typo * Fixed WebUI crash (#633) While eliminating a redundant definition of is_realtime_command(), I inadvertently introduced a recursion due to the similarity of the names "is_realtime_command()" and "is_realtime_cmd()". The solution is to eliminate the latter entirely. * Fix i2s probing hang (#608) * Fix I2S stepper hung just after the completion of motor moving * Fix recompile issue Fixed a problem with the recompile not being recompiled even if the files under the Custom folder are changed. * More comment for macOS in debug.ini * Fix the timing of calling I2S out's exclusion function and reset sequence The reset sequence did not seem to be correct, so I changed it. According to the ESP-IDF PR, the correct sequence is as follows: 1)TX module 2)DMA 3)FIFO https://github.com/espressif/esp-idf/commit/c7f33524b469e75937f003d4c06336bf4694a043#diff-27688c6b3c29373d2a2b142b8471981c * Changed the message level for I2S swtiching from warning to debug * Add some comments * Implement stepping through Motors class (#636) * Implement stepping through Motors class WIP for discussion and review - not ready to merge yet * Document Motor methods and variables .. and remove some unused ones and move some that are subclass-specific * Move position_min/max to Limits.cpp ... and coalesced other uses thereof into a unified scheme. * Call motor ->init() explicitly instead of implicitly This makes it possible to inherit constructors without spurious config messages. * Fixed problems with I2S * Changes in class method override syntax per atlaste * Fixed oops * More Motors simplification a) Eliminated can_home() in favor of a return value from set_homing_mode() b) Eliminated axis_name() in favor of reportAxisNameMsg() * Fixes to RcServo and Trinamic - RC Servo was not handling disable ... probably old issue - Display test after config * More tweaks * Define that variable! * Move functions from Motors.cpp to subclasses Created a Servo base class from which RcServo and Dynamixel2 are derived. This gets the servo update task out of Motors. It also eliminates the need for type_id. Now all of the functions that are specific to particular kinds of motors are within their subclasses * Adding Dynamixel to ABC axes. * Removed second #ifndef SPINDLE_TYPE * Fixed potential leak in Report.cpp as reported by @atlaste * Some servo cleanup. Has errors! * min should be max * Removed test rcservo machine definition. * Removed obsolete #defines in machine defs for RcServo cal Co-authored-by: bdring * Cleaned up AMASS code (#635) * Cleaned up AMASS code More #defines gone 74 lines shorter Tested by comparing the result of original AMASS computation code to the new code with values surrounding all of the cutoff frequencies. * I2SOut tick calculation * Sorted out units for stepper pulse periods I tried to make it clear what the units are at different places in the code, and to use argument datatypes that clearly show the value range at different points, instead of relying on implicit type promotion. Hopefully this will make it easier to understand when, where, and why unit conversions occur. * Update Stepper.h * Deleted AMASS Config.h option ... as it is no longer optional * Use less memory (#644) a) closeFile() now does SD.end() to release memory after running a file from SD. b) Several task stacks are smaller c) All tasks now check their free space if DEBUG_REPORT_STACK_FREE is defined. platformio.ini has a commented-out line that can be uncommented to turn that on. d) Similarly, platformio.ini can turn on DEBUG_REPORT_HEAP_SIZE e) Fixed a small leak that occurred when listing local files. With these changes, the heap size tends to hover around 53K, dropping to about 37K when running a file from SD. * Add coolant pin messages to startup (#647) * Add coolant pin messages to startup Help with user support. * Removing incorrect STEPPER_RESET definition * Fix laser mode startup message * cleanup - coolant_init() will behave as before - update build date - return default machine to test_drive * Move CoreXY out of main Grbl (#653) * Created branch * WIP * Update parallel_delta.cpp * Wip * WIP * Wip * Still working on kinematics - Added an interface into the jogging section * WIP * WIP * wip * WIP * WIP * Wip * WIP * WIP * Wip * Update machine defs * Created branch * WIP * Update parallel_delta.cpp * Wip * WIP * Wip * Still working on kinematics - Added an interface into the jogging section * WIP * WIP * wip * WIP * WIP * Wip * WIP * WIP * Wip * Update machine defs * Machine def change. Moved switches to module 1 * WIP * Cleanup before P.R. - Fixed ranges for delta geometry - Added post homing delay option for servos - renamed and removed old machine defs. * Fixing initialization problem when not in USE_KINEMATICS mode * Fixing Git Mess * Publishing Branch - Not ready yet. Issues with Z axis - Need to add midTbot option * WIP - Seems to be fully functional now. - Need to add midTbot option. * Update CoreXY.cpp * I think it is ready for PR - fixed $RST=# - added midTbot geometry factor * Fine tune midtbot definition * Removed more unneeded corexy code. * Fixed doubled #define in machine def file. * Update after review comments * Added $A AKA Alarms/List command (#654) * Added $A AKA Alarms/List command Similar to $E AKA Errors/List $E used to be AKA ErrorCodes/List Also added $Errors/Verbose setting to display full error text instead of the error number. It defaults to true because it works with every sender I have tried so far - cncjs, UGS, and Chrome GCode Sender. If you have problems with some sender you can set it to false. * Added static_assert per atlaste's comment * Added a default and fixed Authentication issue Co-authored-by: bdring * TMC2130 plotter machine servo config update (#657) * TMC2130 plotter machine servo config update based on Slack conversation https://buildlog.slack.com/archives/CBZKZ8LHL/p1604243530253000 * Update Grbl.h * Trinamic reporting (#656) * Enhanced reporting of errors * Change "motor" to "driver" for clarity. * Added better way to show changed Setting values from Mitch * Update build date * Machine Definition Cleanup (#658) - Removed machine definitions to speed up testing. - Moved 6 pack CS/MS3 pins with other axis pins to help them stay in sync with the aixs letters * Spindle delay and Telnet Fix (#676) * Removed early saving of old state Was causing later tests to be wrong * Update Grbl.h * Update TelnetServer.cpp Remove filtering of '\r' character. * ABC Bresenham counter init fix * Rst responses (#679) * Added verification of changes from $RST command When sending $RST=$ you only get these responses. [MSG:WiFi reset done] [MSG:BT reset done] Added the other things that change. [MSG:WiFi reset done] [MSG:BT reset done] [MSG:Settings reset done] [MSG:Postion offsets reset done] * Update ProcessSettings.cpp * Update Grbl.h * Update ProcessSettings.cpp * Fix Spindle State broken in earlier PR * Update Grbl.h * Spindle and laser (#683) * WIP * Updates * Updates - Added Laser/FullPower - Move some stuff from PWM to Laser * WIP * Used the stop function before resetiing pins. * Updates from discussion - Reset_pins is now deinit() - VFD task is now deleted when ... deinit() - Added a Motor/Disable command * Added Mitch's gambit * Cleanup - Finished VFD - Fixed Settings (Thanks Brian!) - changed task cores. * Update VFDSpindle.cpp * Update Laser.cpp * Fixing reset - gpio_reset_pin sets a pullup, which could turn on a device * Changed Spindle messages to CLIENT_ALL * Update Grbl.h * Updates after review * P.R. Cleanup * Most spindle settings cause a new init() * Laser mode (#692) * Update Machine.h * spindles now say if in laser mode * name fix * Updates * Getting rid of crosstalk * Update PWMSpindle.cpp * Reset some values at spindle init() * Update SettingsDefinitions.cpp * Update Grbl.h * Return to test_drive.h * User macro button (#685) * Test Macro Button Idea * Updates * Formating * Changed macro pin reporting to be a single character * Sd Web UI issues (#698) * Updates * returned reportTaskStackSize(uxHighWaterMark); In a #ifdef DEBUG_TASK_STACK guard * Disallow web commands unless idle or alarm state * merging stuff after review * Handle SD busy state in webserver handler (#697) * Handle SD busy state in webserver handler * Update index.html.gz * Fixed reporting * Add case for SD not enabled. * Prevent Web commands except in idle or alarm * Return authentication to the default Co-authored-by: Mitch Bradley Co-authored-by: Luc <8822552+luc-github@users.noreply.github.com> * Update axis squaring checking (#699) * Reverting some spindle changes... CLIENT_ALL caused queue issues * Rate Adjusting Fix * Fix SD card hanging on bad gcode * Fix hang on error 20 from SD/Run (#701) * Fixed strange WCO values on first load (#702) When loading Grbl_Esp32 into a fresh ESP32, the WCOs would often have strange, very large, values. The problem was the code that tries to propagate data from the old "Eeprom" storage format into the new NVS scheme. The old format had a broken checksum computation that made the checksum so weak that if succeeds about half the time on random data. The solution is to get rid of all that old code. The downside is that migration from a build that uses the old format will lose the WCO values. The user will have to reestablish them. Subsequent updates between different versions that both use the new NVS format will propagate WCO values correctly. * Fixes to homing (#706) * Fixes to homing * Update Grbl.h * Clean up after code review. * Trinamic uart (#700) * WIP * WIP * Updates * Update Grbl.h * Removing some test machine definitions * TMC5160 Drivers were not in tests * Fix a few issues with VFDSpindle critical error handling (#705) If a command is critical and fails to receive a response, it should trigger an Alarm. However, because the critical check was only evaluated if the spindle was not already unresponsive, it meant that a critical command failure would be silently ignored if a non-critical command failed before it (putting the VFDSpindle in unresponsive state). Therefore, I've moved the critical check to occur regardless of whether the spindle was already unresponsive. Second, I believe that setting `sys_rt_exec_alarm` is not sufficient to stop the machine and put it into alarm state. Other alarm conditions (such as hard limits) also run an `mc_reset()` to stop motion first. It appears that without this, motion will not be stopped, and in fact, the alarm appears to get cleared if it occurs during motion! * Update per P.R. #704 on main * Update Motors.cpp * Fix undefined probe reporting if inverted. * Settings filtering via regular expressions (#717) * Settings filtering via regular expressions Implements only the most basic - and the most useful - special characters - ^$.* If the search string does not contain a special character, it is interpreted as before. Otherwise the match is either more strict if anchored by ^ or $, or less strict if it contains a . wildcard or a * repetition. * Commentary * Eliminated . metacharacter * Fix SD/List repetition error (#727) * Fix SD/List repetition error The one line change that actually fixes it is Serial.cpp line 162, where the SD state is compared to "not busy" instead of "is idle", thus also handling the "not present" case. In the process, I converted the "const int SDCARD_ ..." to an SDState enum class, thus proving type safety and eliminating yet another untyped uint8_t . * Updates after testing Co-authored-by: bdring * Fixed RcServo Cals * PWM fix and simplification (#722) * PWM fix and simplification This is an alternative solution to the PWM/ISR/float problem. 1. The set_level() argument is the exact value that is passed to the LEDC Write function. It can be interpreted as the numerator of a rational fraction whose denominator is the max PWM value, i.e. the precision, == 1 << _resolution_bits 2. There is a new denominator() method that returns the precision. 3. The sys_pwm_control(mask, duty, synchronize) function is replaced by two functions sys_analog_all_off() and sys_set_analog(io_num, duty). This closely matches the actual usage. The old routine was called from two places, one where the mask was alway 0xFF, the duty was always 0, and synchronize was always false. That is the one that was troublesome from ISR context. The other call always affected a single pin, so the mask formulation with its loop was useless extra baggage. By splitting into two functions, each one becomes much simpler, thus faster and easier to understand. The "synchronize" argument and associated logic moved out to the caller (GCode.cpp), which more closely reflects the behavioral logic. 4. For symmetry, sys_io_control() was similarly split. 5. sys_calc_pwm_precision() was moved into UserOutput.cpp since is it driver specific, not a general system routine. * Update Grbl.h * Delete template.h Co-authored-by: bdring * TMC2209 Stallguard (#748) * TMC2209 Stallguard - Added StallGuard homing support to TMC2209 (UART) - Killed off TMC2208 for now. Too many conflicts with TMC2209. Will return with Diamond motor class hierarchy - Increase StallGuard setting range for TMC2209. Constrianed in each class to actual limits - Added a machine def to test TMC2209 * Update build date * Web cmd modes (#754) * Update System.cpp * WebCommand with configurable modes * Added a few more ESP commands to work in anu state * Update Grbl.h Co-authored-by: Mitch Bradley * Updates from PWM_ISR_Fix branch (#755) - $Message/Level - ISR safe ledcWrite * Core XY fixes (#756) * Updates for CoreXY * Delete fystec_ant.h * Parking delay fix (#770) * Changed delay type - mc_dwell was causing a recursive loop the overflowed the stack - See https://discord.com/channels/780079161460916227/786061602754396160/809288050387189782 * Changed spindle delays from floats to ints in spindle classes - Used local copies, because I did not want to change/break the existing setting. * Cleaning up parking - Added a coolant delay setting - Made an enum class for the dwell types - Got rid of the safety door sepcific delays * Update Grbl.h * Enable per motor fix (#771) * - moved invert option in front of per motor enables. * Added code to prevent motors_set_disable() from setting values that already exist. * Added the enable delay from PR 720 * Adding a defined default for step enable delay * Fixing feed rates with kinematics and arcs. - Kinematics changes the feed rate and the loop that creates arc was re-using the altered rate. This caused a runaway situation. * SD Upload fix by luc (#779) * Configure motors after I/O pins (#742) So machine definitions can change the SPI pins before we talk to any Trinamic drivers. * 1.0.5 compilation fixes (#782) * Fix compilations error due to new enum in esp32 core release 1.0.5 * Update Grbl.h Co-authored-by: Luc * Introduced delays for enable and direction pins (#720) * Added some timing changes to steppers. Basically this should introduce a small (configurable) delay for enable and direction pins. * Updates after testing * Fixed small casting bug * Fixed subtractions as per @Mitch's comments * Added STEP_PULSE_DELAY handling for compatibility with RMT_STEPS. Functionality should be unchanged. * Updates to help merge Co-authored-by: Stefan de Bruijn Co-authored-by: bdring * Vfd and mc_delay issues. Includes VFD sync speed. (#765) * Added some timing changes to steppers. Basically this should introduce a small (configurable) delay for enable and direction pins. * Updates after testing * Fixed small casting bug * Fixed subtractions as per @Mitch's comments * Fixed busy flag in Stepper.cpp * - Changed mc_dwell to double's, because float's have issues with context switches - Changed spinup/spindown of VFD's to something more sensible - Implemented advanced wait for spinup/spindown of VFD's * Changed machine defs * Implemented spindle speed sync. Made spindle safety polling optional; all the chatter appears to give issues somehow, not sure why. * Made safety-polling optional. * Added 'todo' for actual RPM code. Not sure how this should work though. * Added safety check for unchanged RPM. Changed error output channels. * Enabled the RPM code. * Commented out again. * Changed initialization sequence. Implemented Huanyang initialization sequence. * Made some fixes according to our latest insights in the manual * Fixed docs. * Fixed another bug in the comments * Fixed set_speed RPM. * Fixed huanyang message parsing. Fixed comments. * Forgot a fall-through return. * Updated status message after VFD initializes * Changed output RPM test to output frequency test for sync. * Changed mc_dwell to milliseconds and made it an integer. Fixed bug with negative values. Fixed a dwelling bug in the VFD code, which would dwell indefinitely in cases such as safety door. * Fixed a few small issues during the devt merge. Nothing serious. * Not having critical initialization makes sense for the collet wrench lock thing (or just VFD's that are powered up later) in the sense that initialization should eventually happen to get the right values. Moreover, when the VFD enters an unresponsive state, re-initialization happens eventually. * Changed 50 to 5000 for huanyang frequency calc. * Updates after testing Huanyang * Ran clang-format. Cleaned up the code a bit. Co-authored-by: Stefan de Bruijn Co-authored-by: bdring * Bug fixes (#790) * Fix memory leak when answering webcommand (regression issue) Fix wrong error code on web command (regression issue) * Fix sd code not disabled when SD Card feature is disabled * Use proper error codes / messages * Update Grbl.h * Revert "Use proper error codes / messages" This reverts commit ad49cf8cc1cb2064a8f7687cb4c354eba4bc8172. * Updated error code symbols and text - To be generic file system error Co-authored-by: Luc Co-authored-by: bdring * Get next rmt chan num fix (#793) * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * Update - moved get_next_RMT_chan_num() to constructor - [Discord discussion](https://discord.com/channels/780079161460916227/786364602223951882/817056437016592384) Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser * Rc servo updates (#794) * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * RcServo Updates - Improved disable. It was not always initially set correctly. - Improved calibration. Now a calibration value greater than 1 moves the motor in a positive direction and a value less than 1 moves it in a negative direction regardless of min/max and direction inverts. * Update Grbl.h Update grbl.h Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Mitch Bradley Co-authored-by: Bart Dring Co-authored-by: odaki Co-authored-by: Pete Wildsmith Co-authored-by: Luc <8822552+luc-github@users.noreply.github.com> Co-authored-by: Scott Bezek Co-authored-by: Florian Ragwitz Co-authored-by: Luc Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: JensHauser --- .gitignore | 1 + Grbl_Esp32/src/Config.h | 12 +- Grbl_Esp32/src/Defaults.h | 6 +- Grbl_Esp32/src/Error.cpp | 22 +- Grbl_Esp32/src/Error.h | 20 +- Grbl_Esp32/src/GCode.cpp | 2 +- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Machines/3axis_rs485.h | 59 ----- Grbl_Esp32/src/MotionControl.cpp | 12 +- Grbl_Esp32/src/MotionControl.h | 2 +- Grbl_Esp32/src/Motors/Motors.cpp | 14 +- Grbl_Esp32/src/Motors/Motors.h | 3 +- Grbl_Esp32/src/Motors/RcServo.cpp | 23 +- Grbl_Esp32/src/Motors/StandardStepper.cpp | 20 +- Grbl_Esp32/src/NutsBolts.cpp | 13 +- Grbl_Esp32/src/NutsBolts.h | 5 +- Grbl_Esp32/src/Protocol.cpp | 14 +- Grbl_Esp32/src/Report.cpp | 6 +- Grbl_Esp32/src/SDCard.cpp | 15 +- Grbl_Esp32/src/Serial.cpp | 4 + Grbl_Esp32/src/SettingsDefinitions.cpp | 22 +- Grbl_Esp32/src/SettingsDefinitions.h | 1 + Grbl_Esp32/src/Spindles/H2ASpindle.cpp | 50 ++-- Grbl_Esp32/src/Spindles/H2ASpindle.h | 5 +- Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp | 247 ++++++++++++++++-- Grbl_Esp32/src/Spindles/HuanyangSpindle.h | 13 +- Grbl_Esp32/src/Spindles/Spindle.cpp | 2 +- Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 275 ++++++++++++++------ Grbl_Esp32/src/Spindles/VFDSpindle.h | 14 +- Grbl_Esp32/src/Stepper.cpp | 78 ++++-- Grbl_Esp32/src/WebUI/WebServer.cpp | 9 +- Grbl_Esp32/src/WebUI/WebSettings.cpp | 54 +++- 32 files changed, 703 insertions(+), 322 deletions(-) delete mode 100644 Grbl_Esp32/src/Machines/3axis_rs485.h diff --git a/.gitignore b/.gitignore index dec931d0..44cc0076 100644 --- a/.gitignore +++ b/.gitignore @@ -20,3 +20,4 @@ __vm/ *.vcxproj.filters *.suo Grbl_Esp32.ino.cpp +/packages diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index f53ca288..ffba4148 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -446,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 diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 91c9e396..9c5ac586 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -46,6 +46,10 @@ # 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 @@ -674,4 +678,4 @@ #ifndef DEFAULT_USER_MACRO3 # define DEFAULT_USER_MACRO3 "" -#endif \ No newline at end of file +#endif diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp index 34dd1cd4..c18ef200 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,5 @@ 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::AnotherInterfaceBusy, "Another interface is busy" }, }; diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h index 80f40a39..14b443eb 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 diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 94ea3237..bd1bf2b9 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1435,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; diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 23ae52b5..5a56918b 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 = "20210226"; +const char* const GRBL_VERSION_BUILD = "20210306"; //#include #include diff --git a/Grbl_Esp32/src/Machines/3axis_rs485.h b/Grbl_Esp32/src/Machines/3axis_rs485.h deleted file mode 100644 index 39a565cb..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_NUM_4 // labeled X -#define X_DIRECTION_PIN GPIO_NUM_16 // labeled X -#define Y_STEP_PIN GPIO_NUM_17 // labeled Y -#define Y_DIRECTION_PIN GPIO_NUM_18 // labeled Y -#define Y2_STEP_PIN GPIO_NUM_19 // labeled Y2 -#define Y2_DIRECTION_PIN GPIO_NUM_21 // labeled Y2 -#define Z_STEP_PIN GPIO_NUM_22 // labeled Z -#define Z_DIRECTION_PIN GPIO_NUM_23 // labeled Z - -#define SPINDLE_TYPE SpindleType::H2A -#define VFD_RS485_TXD_PIN GPIO_NUM_13 // RS485 TX -#define VFD_RS485_RTS_PIN GPIO_NUM_15 // RS485 RTS -#define VFD_RS485_RXD_PIN GPIO_NUM_2 // RS485 RX - -#define X_LIMIT_PIN GPIO_NUM_33 -#define Y_LIMIT_PIN GPIO_NUM_32 -#define Y2_LIMIT_PIN GPIO_NUM_35 -#define Z_LIMIT_PIN GPIO_NUM_34 - -// Set $Homing/Cycle0=X and $Homing/Cycle=XY - -#define PROBE_PIN GPIO_NUM_14 // labeled Probe -#define CONTROL_RESET_PIN GPIO_NUM_27 // labeled Reset -#define CONTROL_FEED_HOLD_PIN GPIO_NUM_26 // labeled Hold -#define CONTROL_CYCLE_START_PIN GPIO_NUM_25 // labeled Start - -// #define VFD_DEBUG_MODE diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 5ea64cb2..1b8cda8f 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 @@ -226,12 +226,12 @@ void mc_arc(float* target, } // Execute dwell in seconds. -void mc_dwell(float seconds) { - if (seconds == 0 || 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, DwellMode::Dwell); + return delay_msec(milliseconds, DwellMode::Dwell); } // return true if the mask has exactly one bit set, diff --git a/Grbl_Esp32/src/MotionControl.h b/Grbl_Esp32/src/MotionControl.h index c9177f0b..c7adebc5 100644 --- a/Grbl_Esp32/src/MotionControl.h +++ b/Grbl_Esp32/src/MotionControl.h @@ -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/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index bedfcd0a..4cb5289a 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -478,7 +478,7 @@ uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming) { return can_home; } -void motors_step(uint8_t step_mask, uint8_t dir_mask) { +bool motors_direction(uint8_t dir_mask) { auto n_axis = number_axis->get(); //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "motors_set_direction_pins:0x%02X", onMask); @@ -493,7 +493,17 @@ void motors_step(uint8_t step_mask, uint8_t dir_mask) { myMotor[axis][0]->set_direction(thisDir); myMotor[axis][1]->set_direction(thisDir); } + + return true; + } else { + return false; } +} + +void motors_step(uint8_t step_mask) { + auto n_axis = number_axis->get(); + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "motors_set_direction_pins:0x%02X", onMask); + // Turn on step pulses for motors that are supposed to step now for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { if (bitnum_istrue(step_mask, axis)) { @@ -513,4 +523,4 @@ void motors_unstep() { myMotor[axis][0]->unstep(); myMotor[axis][1]->unstep(); } -} \ No newline at end of file +} diff --git a/Grbl_Esp32/src/Motors/Motors.h b/Grbl_Esp32/src/Motors/Motors.h index 14550f68..d29fa3c4 100644 --- a/Grbl_Esp32/src/Motors/Motors.h +++ b/Grbl_Esp32/src/Motors/Motors.h @@ -40,7 +40,8 @@ void motors_read_settings(); // The return value is a bitmask of axes that can home uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming); void motors_set_disable(bool disable, uint8_t mask = B11111111); // default is all axes -void motors_step(uint8_t step_mask, uint8_t dir_mask); +bool motors_direction(uint8_t dir_mask); +void motors_step(uint8_t step_mask); void motors_unstep(); void servoUpdateTask(void* pvParameters); diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp index 2eefbfc2..bc0fe8b5 100644 --- a/Grbl_Esp32/src/Motors/RcServo.cpp +++ b/Grbl_Esp32/src/Motors/RcServo.cpp @@ -50,7 +50,7 @@ namespace Motors { ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS); ledcAttachPin(_pwm_pin, _channel_num); _current_pwm_duty = 0; - + _disabled = true; config_message(); startUpdateTask(); } @@ -78,10 +78,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); @@ -107,11 +103,6 @@ namespace Motors { if (_disabled) 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 @@ -127,10 +118,14 @@ namespace Motors { } void RcServo::read_settings() { - _pwm_pulse_min = SERVO_MIN_PULSE * rc_servo_cal_min->get(); - _pwm_pulse_max = SERVO_MAX_PULSE * rc_servo_cal_max->get(); + 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/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp index 8e782ba4..3f5a91da 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.cpp +++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp @@ -41,6 +41,9 @@ namespace Motors { StandardStepper::StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin) : Motor(axis_index), _step_pin(step_pin), _dir_pin(dir_pin), _disable_pin(disable_pin) { +#ifdef USE_RMT_STEPS + _rmt_chan_num = get_next_RMT_chan_num(); +#endif } void StandardStepper::init() { @@ -48,9 +51,7 @@ namespace Motors { config_message(); } - void StandardStepper::read_settings() { - init_step_dir_pins(); - } + void StandardStepper::read_settings() { init_step_dir_pins(); } void StandardStepper::init_step_dir_pins() { _invert_step_pin = bitnum_istrue(step_invert_mask->get(), _axis_index); @@ -68,17 +69,13 @@ 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; rmtItem[1].duration1 = 0; - _rmt_chan_num = get_next_RMT_chan_num(); if (_rmt_chan_num == RMT_CHANNEL_MAX) { return; } @@ -126,5 +123,8 @@ namespace Motors { void StandardStepper::set_direction(bool dir) { digitalWrite(_dir_pin, dir ^ _invert_dir_pin); } - void StandardStepper::set_disable(bool disable) { digitalWrite(_disable_pin, disable); } + void StandardStepper::set_disable(bool disable) { + disable ^= step_enable_invert->get(); + digitalWrite(_disable_pin, disable); + } } diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp index 51245465..8039bdd6 100644 --- a/Grbl_Esp32/src/NutsBolts.cpp +++ b/Grbl_Esp32/src/NutsBolts.cpp @@ -112,11 +112,14 @@ void delay_ms(uint16_t ms) { } // Non-blocking delay function used for general operation and suspend features. -void delay_sec(float seconds, DwellMode 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 == DwellMode::Dwell) { protocol_execute_realtime(); @@ -124,11 +127,13 @@ void delay_sec(float seconds, DwellMode mode) { // 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 15705065..77dd45f0 100644 --- a/Grbl_Esp32/src/NutsBolts.h +++ b/Grbl_Esp32/src/NutsBolts.h @@ -26,7 +26,7 @@ // #define true 1 enum class DwellMode : uint8_t { - Dwell = 0, // (Default: Must be zero) + Dwell = 0, // (Default: Must be zero) SysSuspend = 1, //G92.1 (Do not alter value) }; @@ -62,7 +62,6 @@ static inline int toMotor2(int axis) { const double MM_PER_INCH = (25.40); const double INCH_PER_MM = (0.0393701); - // 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) @@ -90,7 +89,7 @@ const double INCH_PER_MM = (0.0393701); 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, DwellMode 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/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index 126015e8..0a331983 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 @@ -193,7 +193,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) { motors_set_disable(true); } @@ -272,7 +272,7 @@ void protocol_exec_rt_system() { } ExecState rt_exec_state; rt_exec_state.value = sys_rt_exec_state.value; // Copy volatile sys_rt_exec_state. - if (rt_exec_state.value != 0 || cycle_stop) { // Test if any bits are on + if (rt_exec_state.value != 0 || cycle_stop) { // Test if any bits are on // Execute system abort. if (rt_exec_state.bit.reset) { sys.abort = true; // Only place this is set true. @@ -664,10 +664,10 @@ static void protocol_exec_rt_suspend() { if (spindle->inLaserMode()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. sys.step_control.updateSpindleRpm = true; - } else { + } else { spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed); // restore delay is done in the spindle class - //delay_sec(spindle_delay_spinup->get(), DwellMode::SysSuspend); + //delay_sec(int32_t(1000.0 * spindle_delay_spinup->get()), DwellMode::SysSuspend); } } } @@ -676,7 +676,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. coolant_set_state(restore_coolant); - delay_sec(coolant_start_delay->get(), DwellMode::SysSuspend); + delay_msec(int32_t(1000.0 * coolant_start_delay->get()), DwellMode::SysSuspend); } } #ifdef PARKING_ENABLE diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 892230e8..b4728154 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -291,10 +291,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); diff --git a/Grbl_Esp32/src/SDCard.cpp b/Grbl_Esp32/src/SDCard.cpp index 32dd95ef..eadac19f 100644 --- a/Grbl_Esp32/src/SDCard.cpp +++ b/Grbl_Esp32/src/SDCard.cpp @@ -18,7 +18,9 @@ along with Grbl. If not, see . */ -#include "SDCard.h" +#include "Config.h" +#ifdef ENABLE_SD_CARD +# include "SDCard.h" File myFile; bool SD_ready_next = false; // Grbl has processed a line and is waiting for another @@ -31,7 +33,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; @@ -41,11 +43,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(); @@ -64,7 +66,7 @@ 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(SDState::BusyPrinting); @@ -94,7 +96,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; @@ -169,3 +171,4 @@ void sd_get_current_filename(char* name) { name[0] = 0; } } +#endif //ENABLE_SD_CARD diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index 9a4b9e4e..c5d0220b 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -159,16 +159,20 @@ void serialCheckTask(void* pvParameters) { 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"); } } +#endif //ENABLE_SD_CARD } } // if something available WebUI::COMMANDS::handle(); diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 8d0aecc2..9d7ad8f6 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -10,6 +10,7 @@ StringSetting* build_info; IntSetting* pulse_microseconds; IntSetting* stepper_idle_lock_time; +IntSetting* direction_delay_microseconds; IntSetting* enable_delay_microseconds; AxisMaskSetting* step_invert_mask; @@ -351,8 +352,7 @@ void make_settings() { 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); + 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, checkSpindleChange); @@ -393,13 +393,17 @@ 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, 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); + 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", 0, 0, 1000); + enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", 0, 0, 1000); // microseconds + + 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); diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index 84c62367..b079786e 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -18,6 +18,7 @@ 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; diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp index b18249e5..a62112fd 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp @@ -24,7 +24,7 @@ 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. */ @@ -80,31 +80,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 +122,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..8243867a 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.h +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.h @@ -29,9 +29,12 @@ namespace Spindles { 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; } }; } diff --git a/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp b/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp index e0172de3..22fddb9d 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,8 +123,30 @@ 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 @@ -151,13 +191,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 +351,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..9c6ee875 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,22 @@ namespace Spindles { int reg; protected: + 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 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_status_ok(ModbusCommand& data) override; + response_parser get_current_rpm(ModbusCommand& data) override; + + bool supports_actual_rpm() const override { return true; } }; } diff --git a/Grbl_Esp32/src/Spindles/Spindle.cpp b/Grbl_Esp32/src/Spindles/Spindle.cpp index f705aea7..b9cfed56 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.cpp +++ b/Grbl_Esp32/src/Spindles/Spindle.cpp @@ -104,4 +104,4 @@ namespace Spindles { void Spindle::deinit() { stop(); } } -Spindles::Spindle* spindle; + Spindles::Spindle* spindle; diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index 76e30c80..a332d0fd 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -1,9 +1,9 @@ /* VFDSpindle.cpp - This is for a VFD based spindles via RS485 Modbus. The details of the - VFD protocol heavily depend on the VFD in question here. We have some - implementations, but if yours is not here, the place to start is the + This is for a VFD based spindles via RS485 Modbus. The details of the + VFD protocol heavily depend on the VFD in question here. We have some + implementations, but if yours is not here, the place to start is the manual. This VFD class implements the modbus functionality. Part of Grbl_ESP32 @@ -26,7 +26,7 @@ Remove power before changing bits. TODO: - - We can report spindle_state and rpm better with VFD's that support + - We can report spindle_state and rpm better with VFD's that support either mode, register RPM or actual RPM. - Destructor should break down the task. - Move min/max RPM to protected members. @@ -34,11 +34,19 @@ */ #include "VFDSpindle.h" +#include + +// Timing and modbus... The manual states that between communications, we should respect a +// silent interval of 3,5 characters. If we received communications between these times, we +// have to assume that the message is broken. We use a poll rate of 250 ms here, which should +// be plenty: assuming 9600 8N1, that's roughly 250 chars. A message of 2x16 chars with 4x4 +// chars buffering is just 40 chars. + const uart_port_t VFD_RS485_UART_PORT = UART_NUM_2; // hard coded for this port right now const int VFD_RS485_BUF_SIZE = 127; -const int VFD_RS485_QUEUE_SIZE = 10; // numv\ber of commands that can be queued up. -const int RESPONSE_WAIT_TICKS = 50; // how long to wait for a response -const int VFD_RS485_POLL_RATE = 200; // in milliseconds between commands +const int VFD_RS485_QUEUE_SIZE = 10; // numv\ber of commands that can be queued up. +const int RESPONSE_WAIT_MILLIS = 1000; // how long to wait for a response in milliseconds +const int VFD_RS485_POLL_RATE = 250; // in milliseconds between commands // OK to change these // #define them in your machine definition file if you want different values @@ -53,11 +61,12 @@ namespace Spindles { // The communications task void VFD::vfd_cmd_task(void* pvParameters) { static bool unresponsive = false; // to pop off a message once each time it becomes unresponsive - static int pollidx = 0; + static int pollidx = -1; // -1 starts the VFD initialization sequence VFD* instance = static_cast(pvParameters); ModbusCommand next_cmd; uint8_t rx_message[VFD_RS485_MAX_MSG_SIZE]; + bool safetyPollingEnabled = instance->safety_polling(); while (true) { response_parser parser = nullptr; @@ -66,10 +75,10 @@ namespace Spindles { // First check if we should ask the VFD for the max RPM value as part of the initialization. We // should also query this is max_rpm is 0, because that means a previous initialization failed: - if (pollidx == 0 || (instance->_max_rpm == 0 && (parser = instance->get_max_rpm(next_cmd)) != nullptr)) { - pollidx = 1; - next_cmd.critical = true; + if ((pollidx < 0 || instance->_max_rpm == 0) && (parser = instance->initialization_sequence(pollidx, next_cmd)) != nullptr) { + next_cmd.critical = false; } else { + pollidx = 1; // Done with initialization. Main sequence. next_cmd.critical = false; } @@ -77,34 +86,39 @@ namespace Spindles { if (parser == nullptr && xQueueReceive(vfd_cmd_queue, &next_cmd, 0) != pdTRUE) { // We poll in a cycle. Note that the switch will fall through unless we encounter a hit. // The weakest form here is 'get_status_ok' which should be implemented if the rest fails. - switch (pollidx) { - case 1: - parser = instance->get_current_rpm(next_cmd); - if (parser) { - pollidx = 2; - break; - } - // fall through intentionally: - case 2: - parser = instance->get_current_direction(next_cmd); - if (parser) { - pollidx = 3; - break; - } - // fall through intentionally: - case 3: - parser = instance->get_status_ok(next_cmd); - pollidx = 1; + if (instance->_syncing) { + parser = instance->get_current_rpm(next_cmd); + } else if (safetyPollingEnabled) { + switch (pollidx) { + case 1: + parser = instance->get_current_rpm(next_cmd); + if (parser) { + pollidx = 2; + break; + } + // fall through intentionally: + case 2: + parser = instance->get_current_direction(next_cmd); + if (parser) { + pollidx = 3; + break; + } + // fall through intentionally: + case 3: + default: + parser = instance->get_status_ok(next_cmd); + pollidx = 1; - // we could complete this in case parser == nullptr with some ifs, but let's - // just keep it easy and wait an iteration. - break; + // we could complete this in case parser == nullptr with some ifs, but let's + // just keep it easy and wait an iteration. + break; + } } // If we have no parser, that means get_status_ok is not implemented (and we have // nothing resting in our queue). Let's fall back on a simple continue. if (parser == nullptr) { - vTaskDelay(VFD_RS485_POLL_RATE); + vTaskDelay(VFD_RS485_POLL_RATE / portTICK_PERIOD_MS); continue; // main while loop } } @@ -120,7 +134,7 @@ namespace Spindles { next_cmd.msg[next_cmd.tx_length - 1] = (crc16 & 0xFF00) >> 8; next_cmd.msg[next_cmd.tx_length - 2] = (crc16 & 0xFF); -#ifdef VFD_DEBUG_MODE +#ifdef VFD_DEBUG_MODE2 if (parser == nullptr) { report_hex_msg(next_cmd.msg, "RS485 Tx: ", next_cmd.tx_length); } @@ -130,12 +144,36 @@ namespace Spindles { // Assume for the worst, and retry... int retry_count = 0; for (; retry_count < MAX_RETRIES; ++retry_count) { - // Flush the UART and write the data: + // Flush the UART: uart_flush(VFD_RS485_UART_PORT); + + // Write the data: uart_write_bytes(VFD_RS485_UART_PORT, reinterpret_cast(next_cmd.msg), next_cmd.tx_length); + uart_wait_tx_done(VFD_RS485_UART_PORT, RESPONSE_WAIT_MILLIS / portTICK_PERIOD_MS); // Read the response - uint16_t read_length = uart_read_bytes(VFD_RS485_UART_PORT, rx_message, next_cmd.rx_length, RESPONSE_WAIT_TICKS); + uint16_t read_length = 0; + uint16_t current_read = + uart_read_bytes(VFD_RS485_UART_PORT, rx_message, next_cmd.rx_length, RESPONSE_WAIT_MILLIS / portTICK_PERIOD_MS); + read_length += current_read; + + // Apparently some Huanyang report modbus errors in the correct way, and the rest not. Sigh. + // Let's just check for the condition, and truncate the first byte. + if (read_length > 0 && VFD_RS485_ADDR != 0 && rx_message[0] == 0) { + memmove(rx_message + 1, rx_message, read_length - 1); + } + + while (read_length < next_cmd.rx_length && current_read > 0) { + // Try to read more; we're not there yet... + current_read = uart_read_bytes(VFD_RS485_UART_PORT, + rx_message + read_length, + next_cmd.rx_length - read_length, + RESPONSE_WAIT_MILLIS / portTICK_PERIOD_MS); + read_length += current_read; + } + if (current_read < 0) { + read_length = 0; + } // Generate crc16 for the response: auto crc16response = ModRTU_CRC(rx_message, next_cmd.rx_length - 2); @@ -145,20 +183,30 @@ namespace Spindles { rx_message[read_length - 1] == (crc16response & 0xFF00) >> 8 && // check CRC byte 1 rx_message[read_length - 2] == (crc16response & 0xFF)) { // check CRC byte 1 - // success + // Success unresponsive = false; retry_count = MAX_RETRIES + 1; // stop retry'ing // Should we parse this? - if (parser != nullptr && !parser(rx_message, instance)) { + if (parser != nullptr) { + if (parser(rx_message, instance)) { + // If we're initializing, move to the next initialization command: + if (pollidx < 0) { + --pollidx; + } + } else { + // If we were initializing, move back to where we started. #ifdef VFD_DEBUG_MODE - report_hex_msg(next_cmd.msg, "RS485 Tx: ", next_cmd.tx_length); - report_hex_msg(rx_message, "RS485 Rx: ", read_length); + // Parsing failed + report_hex_msg(next_cmd.msg, "RS485 Tx: ", next_cmd.tx_length); + report_hex_msg(rx_message, "RS485 Rx: ", read_length); #endif - // Not succesful! Now what? - unresponsive = true; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); + // Not succesful! Now what? + unresponsive = true; + pollidx = -1; // Re-initializing the VFD seems like a plan + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); + } } } else { #ifdef VFD_DEBUG_MODE @@ -184,7 +232,7 @@ namespace Spindles { // Wait a bit before we retry. Set the delay to poll-rate. Not sure // if we should use a different value... - vTaskDelay(VFD_RS485_POLL_RATE); + vTaskDelay(VFD_RS485_POLL_RATE / portTICK_PERIOD_MS); static UBaseType_t uxHighWaterMark = 0; reportTaskStackSize(uxHighWaterMark); @@ -193,17 +241,18 @@ namespace Spindles { if (retry_count == MAX_RETRIES) { if (!unresponsive) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); unresponsive = true; + pollidx = -1; } if (next_cmd.critical) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Error, "Critical Spindle RS485 Unresponsive"); mc_reset(); sys_rt_exec_alarm = ExecAlarm::SpindleControl; } } - vTaskDelay(VFD_RS485_POLL_RATE); // TODO: What is the best value here? + vTaskDelay(VFD_RS485_POLL_RATE / portTICK_PERIOD_MS); } } @@ -217,7 +266,9 @@ namespace Spindles { } void VFD::init() { - vfd_ok = false; // initialize + vfd_ok = false; // initialize + _sync_rpm = 0; + _syncing = false; grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Initializing RS485 VFD spindle"); @@ -266,6 +317,15 @@ namespace Spindles { return; } + // We have to initialize the constants before starting the task: + is_reversable = true; // these VFDs are always reversable + use_delays = true; + vfd_ok = true; + + // Initially we initialize this to 0; over time, we might poll better information from the VFD. + _current_rpm = 0; + _current_state = SpindleState::Disable; + // 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 vfd_cmd_queue = xQueueCreate(VFD_RS485_QUEUE_SIZE, sizeof(ModbusCommand)); @@ -280,14 +340,6 @@ namespace Spindles { _task_running = true; } - is_reversable = true; // these VFDs are always reversable - use_delays = true; - vfd_ok = true; - - // Initially we initialize this to 0; over time, we might poll better information from the VFD. - _current_rpm = 0; - _current_state = SpindleState::Disable; - config_message(); } @@ -323,6 +375,7 @@ namespace Spindles { pins_settings_ok = false; } + // Use config for initial RPM values: _min_rpm = rpm_min->get(); _max_rpm = rpm_max->get(); @@ -346,32 +399,93 @@ namespace Spindles { return; // Block during abort. } - bool critical = (sys.state == State::Cycle || state != SpindleState::Disable); + bool shouldWait = state != _current_state || state != SpindleState::Disable; + bool critical = (sys.state == State::Cycle || state != SpindleState::Disable); + + int32_t delayMillis = 1000; 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 set_rpm(rpm); - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin1"); - if (state == SpindleState::Disable) { sys.spindle_speed = 0; - delay(_spindown_delay); + delayMillis = _spindown_delay; + rpm = 0; } else { - delay(_spinup_delay); + delayMillis = _spinup_delay; + } + + if (_current_state != state && !supports_actual_rpm()) { + delay(delayMillis); } - - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin2"); } else { if (_current_rpm != rpm) { set_rpm(rpm); + + if (rpm > _current_rpm) { + delayMillis = _spinup_delay; + } else { + delayMillis = _spindown_delay; + } } } - _current_state = state; // store locally for faster get_state() + if (shouldWait) { + if (supports_actual_rpm()) { + _syncing = true; - sys.report_ovr_counter = 0; // Set to report change immediately + // Allow 2.5% difference from what we asked for. Should be fine. + uint32_t drpm = (_max_rpm - _min_rpm) / 40; + if (drpm < 100) { + drpm = 100; + } // Just a sanity check + + auto minRpmAllowed = _current_rpm > drpm ? (_current_rpm - drpm) : 0; + auto maxRpmAllowed = _current_rpm + drpm; + + int unchanged = 0; + const int limit = 20; // 20 * 0.5s = 10 sec + auto last = _sync_rpm; + + while ((_sync_rpm < minRpmAllowed || _sync_rpm > maxRpmAllowed) && unchanged < limit) { +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Syncing RPM. Requested %d, current %d", int(rpm), int(_sync_rpm)); +#endif + if (!mc_dwell(500)) { + // Something happened while we were dwelling, like a safety door. + unchanged = limit; + last = _sync_rpm; + break; + } + + if (_sync_rpm == last) { + ++unchanged; + } else { + unchanged = 0; + } + last = _sync_rpm; + } + + if (unchanged == limit) { + grbl_msg_sendf(CLIENT_ALL, + MsgLevel::Error, + "Critical Spindle RS485 did not reach speed %d. Reported speed is %d rpm.", + rpm, + _sync_rpm); + mc_reset(); + sys_rt_exec_alarm = ExecAlarm::SpindleControl; + } + + _syncing = false; + } else { + delay(delayMillis); + } + } + + _current_state = state; // store locally for faster get_state() + sys.report_ovr_counter = 0; // Set to report change immediately return; } @@ -407,26 +521,32 @@ namespace Spindles { return 0; } -#ifdef VFD_DEBUG_MODE - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); -#endif - // apply override rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (uint8_t percent) - // apply limits - if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) { - rpm = _max_rpm; - } else if (rpm != 0 && rpm <= _min_rpm) { - rpm = _min_rpm; + if (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); + rpm = constrain(rpm, _min_rpm, _max_rpm); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed changed to %d", rpm); } + // apply limits + // if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) { + // rpm = _max_rpm; + // } else if (rpm != 0 && rpm <= _min_rpm) { + // rpm = _min_rpm; + // } + sys.spindle_speed = rpm; if (rpm == _current_rpm) { // prevent setting same RPM twice return rpm; } +#ifdef VFD_DEBUG_MODE2 + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); +#endif + _current_rpm = rpm; // TODO add the speed modifiers override, linearization, etc. @@ -434,9 +554,10 @@ namespace Spindles { ModbusCommand rpm_cmd; rpm_cmd.msg[0] = VFD_RS485_ADDR; + set_speed_command(rpm, rpm_cmd); - rpm_cmd.critical = false; + rpm_cmd.critical = (rpm == 0); if (xQueueSend(vfd_cmd_queue, &rpm_cmd, 0) != pdTRUE) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full"); @@ -445,7 +566,7 @@ namespace Spindles { return rpm; } - void VFD::stop() { set_mode(SpindleState::Disable, false); } + void VFD::stop() { set_mode(SpindleState::Disable, true); } // state is cached rather than read right now to prevent delays SpindleState VFD::get_state() { return _current_state; } @@ -471,4 +592,4 @@ namespace Spindles { return crc; } -} \ No newline at end of file +} diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.h b/Grbl_Esp32/src/Spindles/VFDSpindle.h index 7e81443d..2661d246 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 @@ -29,7 +29,7 @@ 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(); @@ -66,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; @@ -82,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(); @@ -92,4 +96,4 @@ namespace Spindles { virtual ~VFD() {} }; -} \ No newline at end of file +} diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index d4919614..c0d81a18 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -25,6 +25,8 @@ #include "Grbl.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 // never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). @@ -58,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 @@ -81,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. @@ -182,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 @@ -196,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; } /** @@ -221,14 +222,42 @@ void IRAM_ATTR onStepperDriverTimer( static void stepper_pulse_func() { auto n_axis = number_axis->get(); - motors_step(st.step_outbits, st.dir_outbits); + 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; + } + } + } // 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 // at the end of this routine without incurring another interrupt. // This is unnecessary with RMT and I2S stepping since both of // those methods time the turn off automatically. + // + // NOTE: We could use direction_pulse_start_time + wait_direction, but let's play it safe uint64_t step_pulse_start_time = esp_timer_get_time(); + motors_step(st.step_outbits); // If there is no step segment, attempt to pop one from the stepper buffer if (st.exec_segment == NULL) { @@ -323,6 +352,8 @@ static void stepper_pulse_func() { } void stepper_init() { + busy.store(false); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Axis count %d", number_axis->get()); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s", stepper_names[current_stepper]); @@ -359,13 +390,20 @@ void st_wake_up() { // Enable stepper drivers. motors_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(); } @@ -390,7 +428,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? @@ -400,7 +437,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) && diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index 311c980a..b4a1cef5 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -480,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"); @@ -491,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 @@ -512,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":""); } } diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index 84e2ec18..f2a312f2 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -302,11 +302,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; @@ -338,11 +338,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'); @@ -686,16 +686,16 @@ namespace WebUI { 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; } @@ -764,18 +764,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."); } @@ -788,10 +788,10 @@ namespace WebUI { 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(""); @@ -806,9 +806,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());