1
0
mirror of https://github.com/bdring/Grbl_Esp32.git synced 2025-09-03 03:13:25 +02:00
* Fixed various small bugs (#605)

* Fixed various small bugs

* Fixed potential cast bug

* Fixed double reporting of errors

Co-authored-by: Stefan de Bruijn <stefan@nubilosoft.com>

* 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 <stefan@nubilosoft.com>

* Fix step leakage with inverted steps (#570)

* Fix step leakage with inverted steps

* Update build date for merge

Co-authored-by: Bart Dring <bdring@buildlog.net>

Co-authored-by: Stefan de Bruijn <atlaste@users.noreply.github.com>
Co-authored-by: Stefan de Bruijn <stefan@nubilosoft.com>
Co-authored-by: Mitch Bradley <wmb@firmworks.com>
Co-authored-by: Bart Dring <bdring@buildlog.net>

* 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 <atlaste@users.noreply.github.com>
Co-authored-by: Stefan de Bruijn <stefan@nubilosoft.com>
Co-authored-by: Mitch Bradley <wmb@firmworks.com>
Co-authored-by: Bart Dring <bdring@buildlog.net>

* Basic testing Complete

* Made state variable volatile.

* Homing cycle settings (#613)

* Initial Tests Complete

* Update Grbl.h

* Update variables

Co-authored-by: Mitch Bradley <wmb@firmworks.com>

* 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 <stefan@nubilosoft.com>

* $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=<whatever>

$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
c7f33524b4 (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 <barton.dring@gmail.com>

* 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 <barton.dring@gmail.com>

* 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 <wmb@firmworks.com>
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 <barton.dring@gmail.com>

* 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 <barton.dring@gmail.com>

* 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 <wmb@firmworks.com>

* 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.

Co-authored-by: Stefan de Bruijn <atlaste@users.noreply.github.com>
Co-authored-by: Stefan de Bruijn <stefan@nubilosoft.com>
Co-authored-by: Mitch Bradley <wmb@firmworks.com>
Co-authored-by: Bart Dring <bdring@buildlog.net>
Co-authored-by: odaki <odaki@mars.dti.ne.jp>
Co-authored-by: Pete Wildsmith <pete@weargoggles.co.uk>
Co-authored-by: Luc <8822552+luc-github@users.noreply.github.com>
Co-authored-by: Scott Bezek <scottbez1@gmail.com>
Co-authored-by: Florian Ragwitz <florian.ragwitz@gmail.com>
This commit is contained in:
bdring
2021-02-20 12:37:50 -06:00
committed by GitHub
parent 40fcdf45bc
commit 549917035c
18 changed files with 160 additions and 69 deletions

View File

@@ -69,7 +69,8 @@ bool user_defined_homing(uint8_t cycle_mask) {
// check for multi axis homing per cycle ($Homing/Cycle0=XY type)...not allowed in CoreXY
bool setting_error = false;
for (int cycle = 0; cycle < 3; cycle++) {
auto n_axis = number_axis->get();
for (int cycle = 0; cycle < n_axis; cycle++) {
if (numberOfSetBits(homing_cycle[cycle]->get()) > 1) {
grbl_msg_sendf(CLIENT_SERIAL,
MsgLevel::Info,
@@ -90,7 +91,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
pl_data->motion.systemMotion = 1;
pl_data->motion.noFeedOverride = 1;
uint8_t cycle_count = (cycle_mask == 0) ? 3 : 1; // if we have a cycle_mask, we are only going to do one axis
uint8_t cycle_count = (cycle_mask == 0) ? n_axis : 1; // if we have a cycle_mask, we are only going to do one axis
AxisMask mask = 0;
for (int cycle = 0; cycle < cycle_count; cycle++) {
@@ -105,7 +106,7 @@ bool user_defined_homing(uint8_t cycle_mask) {
mask = motors_set_homing_mode(mask, true); // non standard homing motors will do their own thing and get removed from the mask
for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) {
for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) {
if (bit(axis) == mask) {
// setup for the homing of this axis
bool approach = true;
@@ -129,7 +130,9 @@ bool user_defined_homing(uint8_t cycle_mask) {
approach ? target[axis] = max_travel : target[axis] = -max_travel;
}
target[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS);
for (int axis = Z_AXIS; axis < n_axis; axis++) {
target[axis] = system_convert_axis_steps_to_mpos(sys_position, axis);
}
// convert back to motor steps
inverse_kinematics(target);
@@ -217,7 +220,10 @@ bool user_defined_homing(uint8_t cycle_mask) {
last_cartesian[X_AXIS] = target[X_AXIS];
last_cartesian[Y_AXIS] = target[Y_AXIS];
last_cartesian[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS);
for (int axis = Z_AXIS; axis < n_axis; axis++) {
last_cartesian[axis] = system_convert_axis_steps_to_mpos(sys_position, axis);
}
// convert to motors
inverse_kinematics(target);
@@ -239,15 +245,15 @@ bool user_defined_homing(uint8_t cycle_mask) {
// This function is used by Grbl convert Cartesian to motors
// this does not do any motion control
void inverse_kinematics(float* position) {
float motors[3];
float motors[MAX_N_AXIS];
motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS];
motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS];
motors[Z_AXIS] = position[Z_AXIS];
position[0] = motors[0];
position[1] = motors[1];
position[2] = motors[2];
position[X_AXIS] = motors[X_AXIS];
position[Y_AXIS] = motors[Y_AXIS];
// Z and higher just pass through unchanged
}
// Inverse Kinematics calculates motor positions from real world cartesian positions
@@ -268,7 +274,11 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS];
motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_AXIS];
motors[Z_AXIS] = target[Z_AXIS];
auto n_axis = number_axis->get();
for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) {
motors[axis] = target[axis];
}
float motor_distance = three_axis_dist(motors, last_motors);
@@ -283,14 +293,36 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
// motors -> cartesian
void forward_kinematics(float* position) {
float calc_fwd[MAX_N_AXIS];
float calc_fwd[MAX_N_AXIS];
float wco[MAX_N_AXIS];
float print_position[N_AXIS];
int32_t current_position[N_AXIS]; // Copy current state of the system position variable
memcpy(current_position, sys_position, sizeof(sys_position));
system_convert_array_steps_to_mpos(print_position, current_position);
// determine the Work Coordinate Offsets for each axis
auto n_axis = number_axis->get();
for (int axis = 0; axis < n_axis; axis++) {
// Apply work coordinate offsets and tool length offset to current position.
wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis];
if (axis == TOOL_LENGTH_OFFSET_AXIS) {
wco[axis] += gc_state.tool_length_offset;
}
}
// apply the forward kinemetics to the machine coordinates
// https://corexy.com/theory.html
calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]);
calc_fwd[Y_AXIS] = 0.5 * (position[X_AXIS] - position[Y_AXIS]);
//calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]);
calc_fwd[X_AXIS] = ((0.5 * (print_position[X_AXIS] + print_position[Y_AXIS]) * geometry_factor) - wco[X_AXIS]);
calc_fwd[Y_AXIS] = ((0.5 * (print_position[X_AXIS] - print_position[Y_AXIS])) - wco[Y_AXIS]);
position[X_AXIS] = calc_fwd[X_AXIS];
position[Y_AXIS] = calc_fwd[Y_AXIS];
for (int axis = 0; axis < n_axis; axis++) {
if (axis > Y_AXIS) { // for axes above Y there is no kinematics
calc_fwd[axis] = print_position[axis] - wco[axis];
}
position[axis] = calc_fwd[axis]; // this is the value returned to reporting
}
}
bool kinematics_pre_homing(uint8_t cycle_mask) {
@@ -298,9 +330,10 @@ bool kinematics_pre_homing(uint8_t cycle_mask) {
}
void kinematics_post_homing() {
gc_state.position[X_AXIS] = last_cartesian[X_AXIS];
gc_state.position[Y_AXIS] = last_cartesian[Y_AXIS];
gc_state.position[Z_AXIS] = last_cartesian[Z_AXIS];
auto n_axis = number_axis->get();
for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) {
gc_state.position[axis] = last_cartesian[axis];
}
}
// this is used used by Grbl soft limits to see if the range of the machine is exceeded.

View File

@@ -258,11 +258,6 @@ static const uint8_t NHomingLocateCycle = 1; // Integer (1-128)
// previous tool path, as if nothing happened.
#define ENABLE_SAFETY_DOOR_INPUT_PIN // ESP32 Leave this enabled for now .. code for undefined not ready
// After the safety door switch has been toggled and restored, this setting sets the power-up delay
// between restoring the spindle and coolant and resuming the cycle.
const double SAFETY_DOOR_SPINDLE_DELAY = 4.0; // Float (seconds)
const double SAFETY_DOOR_COOLANT_DELAY = 1.0; // Float (seconds)
// Inverts select limit pin states based on the following mask. This effects all limit pin functions,
// such as hard limits and homing. However, this is different from overall invert limits setting.
// This build option will invert only the limit pins defined here, and then the invert limits setting
@@ -582,8 +577,8 @@ const int DEBOUNCE_PERIOD = 32; // in milliseconds default 32 microseconds
// Configure options for the parking motion, if enabled.
#define PARKING_AXIS Z_AXIS // Define which axis that performs the parking motion
const double PARKING_TARGET = -5.0; // Parking axis target. In mm, as machine coordinate.
const double PARKING_RATE = 500.0; // Parking fast rate after pull-out in mm/min.
const double PARKING_PULLOUT_RATE = 100.0; // Pull-out/plunge slow feed rate in mm/min.
const double PARKING_RATE = 800.0; // Parking fast rate after pull-out in mm/min.
const double PARKING_PULLOUT_RATE = 250.0; // Pull-out/plunge slow feed rate in mm/min.
const double PARKING_PULLOUT_INCREMENT = 5.0; // Spindle pull-out and plunge distance in mm. Incremental distance.
// Must be positive value or equal to zero.

View File

@@ -42,6 +42,10 @@
# define DEFAULT_STEP_PULSE_MICROSECONDS 3 // $0
#endif
#ifndef DEFAULT_STEP_ENABLE_DELAY
# define DEFAULT_STEP_ENABLE_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
@@ -187,6 +191,10 @@
# define DEFAULT_SPINDLE_DELAY_SPINUP 0
#endif
#ifndef DEFAULT_COOLANT_DELAY_TURNON
# define DEFAULT_COOLANT_DELAY_TURNON 1.0
#endif
#ifndef DEFAULT_SPINDLE_DELAY_SPINDOWN
# define DEFAULT_SPINDLE_DELAY_SPINDOWN 0
#endif

View File

@@ -39,13 +39,10 @@ void grbl_init() {
#endif
settings_init(); // Load Grbl settings from non-volatile storage
stepper_init(); // Configure stepper pins and interrupt timers
init_motors();
system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files)
init_motors();
memset(sys_position, 0, sizeof(sys_position)); // Clear machine position.
#ifdef USE_MACHINE_INIT
machine_init(); // user supplied function for special initialization
#endif
machine_init(); // weak definition in Grbl.cpp does nothing
// Initialize system state.
#ifdef FORCE_INITIALIZATION_ALARM
// Force Grbl into an ALARM state upon a power-cycle or hard reset.
@@ -117,6 +114,8 @@ void run_once() {
protocol_main_loop();
}
void __attribute__((weak)) machine_init() {}
/*
setup() and loop() in the Arduino .ino implements this control flow:

View File

@@ -21,9 +21,8 @@
*/
// Grbl versioning system
const char* const GRBL_VERSION = "1.3a";
const char* const GRBL_VERSION_BUILD = "20210203";
const char* const GRBL_VERSION_BUILD = "20210218";
//#include <sdkconfig.h>
#include <Arduino.h>
@@ -94,8 +93,7 @@ void run_once();
// Called if USE_MACHINE_INIT is defined
void machine_init();
// Called if USE_CUSTOM_HOMING is defined
bool user_defined_homing(uint8_t cycle_mask);
bool user_defined_homing(uint8_t cycle_mask); // weak definition in Limits.cpp
// Called if USE_KINEMATICS is defined
@@ -106,7 +104,7 @@ uint8_t kinematic_limits_check(float* target);
// Called if USE_FWD_KINEMATICS is defined
void inverse_kinematics(float* position); // used to return a converted value
void forward_kinematics(float* position);
void forward_kinematics(float* position); // weak definition in Report.cpp
// Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined
void user_defined_macro(uint8_t index);

View File

@@ -404,3 +404,7 @@ bool limitsCheckTravel(float* target) {
}
return false;
}
bool __attribute__((weak)) user_defined_homing(uint8_t cycle_mask) {
return false;
}

View File

@@ -184,8 +184,9 @@ void mc_arc(float* target,
float cos_Ti;
float r_axisi;
uint16_t i;
uint8_t count = 0;
for (i = 1; i < segments; i++) { // Increment (segments-1).
uint8_t count = 0;
float original_feedrate = pl_data->feed_rate; // Kinematics may alter the feedrate, so save an original copy
for (i = 1; i < segments; i++) { // Increment (segments-1).
if (count < N_ARC_CORRECTION) {
// Apply vector rotation matrix. ~40 usec
r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
@@ -206,6 +207,7 @@ void mc_arc(float* target,
position[axis_1] = center_axis1 + r_axis1;
position[axis_linear] += linear_per_segment;
#ifdef USE_KINEMATICS
pl_data->feed_rate = original_feedrate; // This restores the feedrate kinematics may have altered
mc_line_kins(position, pl_data, previous_position);
previous_position[axis_0] = position[axis_0];
previous_position[axis_1] = position[axis_1];
@@ -225,11 +227,11 @@ void mc_arc(float* target,
// Execute dwell in seconds.
void mc_dwell(float seconds) {
if (sys.state == State::CheckMode) {
if (seconds == 0 || sys.state == State::CheckMode) {
return;
}
protocol_buffer_synchronize();
delay_sec(seconds, DELAY_MODE_DWELL);
delay_sec(seconds, DwellMode::Dwell);
}
// return true if the mask has exactly one bit set,
@@ -283,11 +285,11 @@ static bool axis_is_squared(uint8_t axis_mask) {
// executing the homing cycle. This prevents incorrect buffered plans after homing.
void mc_homing_cycle(uint8_t cycle_mask) {
bool no_cycles_defined = true;
#ifdef USE_CUSTOM_HOMING
if (user_defined_homing(cycle_mask)) {
return;
}
#endif
// This give kinematics a chance to do something before normal homing
// if it returns true, the homing is canceled.
#ifdef USE_KINEMATICS

View File

@@ -413,7 +413,19 @@ void init_motors() {
}
void motors_set_disable(bool disable, uint8_t mask) {
static bool previous_state = true;
static bool prev_disable = true;
static uint8_t prev_mask = 0;
if ((disable == prev_disable) && (mask == prev_mask)) {
return;
}
prev_disable = disable;
prev_mask = mask;
if (step_enable_invert->get()) {
disable = !disable; // Apply pin invert.
}
// now loop through all the motors to see if they can individually disable
auto n_axis = number_axis->get();
@@ -425,11 +437,19 @@ void motors_set_disable(bool disable, uint8_t mask) {
}
}
// invert only inverts the global stepper disable pin.
if (step_enable_invert->get()) {
disable = !disable; // Apply pin invert.
}
// global disable.
digitalWrite(STEPPERS_DISABLE_PIN, disable);
// Add an optional delay for stepper drivers. that need time
// Some need time after the enable before they can step.
auto wait_disable_change = enable_delay_microseconds->get();
if (wait_disable_change != 0) {
auto disable_start_time = esp_timer_get_time() + wait_disable_change;
while ((esp_timer_get_time() - disable_start_time) < 0) {
NOP();
}
}
}
void motors_read_settings() {

View File

@@ -112,15 +112,15 @@ void delay_ms(uint16_t ms) {
}
// Non-blocking delay function used for general operation and suspend features.
void delay_sec(float seconds, uint8_t mode) {
void delay_sec(float seconds, DwellMode mode) {
uint16_t i = ceil(1000 / DWELL_TIME_STEP * seconds);
while (i-- > 0) {
if (sys.abort) {
return;
}
if (mode == DELAY_MODE_DWELL) {
if (mode == DwellMode::Dwell) {
protocol_execute_realtime();
} else { // DELAY_MODE_SYS_SUSPEND
} else { // DwellMode::SysSuspend
// Execute rt_system() only to avoid nesting suspend loops.
protocol_exec_rt_system();
if (sys.suspend.bit.restartRetract) {

View File

@@ -25,6 +25,11 @@
// #define false 0
// #define true 1
enum class DwellMode : uint8_t {
Dwell = 0, // (Default: Must be zero)
SysSuspend = 1, //G92.1 (Do not alter value)
};
const double SOME_LARGE_VALUE = 1.0E+38;
// Axis array index values. Must start with 0 and be continuous.
@@ -57,8 +62,6 @@ static inline int toMotor2(int axis) {
const double MM_PER_INCH = (25.40);
const double INCH_PER_MM = (0.0393701);
const int DELAY_MODE_DWELL = 0;
const int DELAY_MODE_SYS_SUSPEND = 1;
// Useful macros
#define clear_vector(a) memset(a, 0, sizeof(a))
@@ -87,7 +90,7 @@ const int DELAY_MODE_SYS_SUSPEND = 1;
uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr);
// Non-blocking delay function used for general operation and suspend features.
void delay_sec(float seconds, uint8_t mode);
void delay_sec(float seconds, DwellMode mode);
// Delays variable-defined milliseconds. Compiler compatibility fix for _delay_ms().
void delay_ms(uint16_t ms);

View File

@@ -664,9 +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);
delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SYS_SUSPEND);
// restore delay is done in the spindle class
//delay_sec(spindle_delay_spinup->get(), DwellMode::SysSuspend);
}
}
}
@@ -675,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(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND);
delay_sec(coolant_start_delay->get(), DwellMode::SysSuspend);
}
}
#ifdef PARKING_ENABLE

View File

@@ -659,13 +659,11 @@ void report_realtime_status(uint8_t client) {
}
}
}
forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version
// Report machine position
if (bit_istrue(status_mask->get(), RtStatus::Position)) {
strcat(status, "|MPos:");
} else {
#ifdef USE_FWD_KINEMATICS
forward_kinematics(print_position);
#endif
strcat(status, "|WPos:");
}
report_util_axis_values(print_position, temp);
@@ -957,3 +955,5 @@ void reportTaskStackSize(UBaseType_t& saved) {
}
#endif
}
void __attribute__((weak)) forward_kinematics(float* position) {} // This version does nothing. Make your own to do something with it

View File

@@ -10,6 +10,7 @@ StringSetting* build_info;
IntSetting* pulse_microseconds;
IntSetting* stepper_idle_lock_time;
IntSetting* enable_delay_microseconds;
AxisMaskSetting* step_invert_mask;
AxisMaskSetting* dir_invert_mask;
@@ -46,6 +47,7 @@ FloatSetting* rpm_max;
FloatSetting* rpm_min;
FloatSetting* spindle_delay_spinup;
FloatSetting* spindle_delay_spindown;
FloatSetting* coolant_start_delay;
FlagSetting* spindle_enbl_off_with_zero_speed;
FlagSetting* spindle_enable_invert;
FlagSetting* spindle_output_invert;
@@ -345,8 +347,12 @@ void make_settings() {
spindle_pwm_freq = new FloatSetting(EXTENDED, WG, "33", "Spindle/PWM/Frequency", DEFAULT_SPINDLE_FREQ, 0, 100000, checkSpindleChange);
spindle_output_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/PWM/Invert", DEFAULT_INVERT_SPINDLE_OUTPUT_PIN, checkSpindleChange);
spindle_delay_spinup = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30);
spindle_delay_spindown = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30);
spindle_delay_spinup =
new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange);
spindle_delay_spindown =
new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange);
coolant_start_delay =
new FloatSetting(EXTENDED, WG, NULL, "Coolant/Delay/TurnOn", DEFAULT_COOLANT_DELAY_TURNON, 0, 30);
spindle_enbl_off_with_zero_speed =
new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED, checkSpindleChange);
@@ -394,6 +400,7 @@ void make_settings() {
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);
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);

View File

@@ -18,6 +18,7 @@ extern StringSetting* build_info;
extern IntSetting* pulse_microseconds;
extern IntSetting* stepper_idle_lock_time;
extern IntSetting* enable_delay_microseconds;
extern AxisMaskSetting* step_invert_mask;
extern AxisMaskSetting* dir_invert_mask;
@@ -48,6 +49,7 @@ extern FloatSetting* rpm_max;
extern FloatSetting* rpm_min;
extern FloatSetting* spindle_delay_spinup;
extern FloatSetting* spindle_delay_spindown;
extern FloatSetting* coolant_start_delay;
extern FlagSetting* spindle_enbl_off_with_zero_speed;
extern FlagSetting* spindle_enable_invert;
extern FlagSetting* spindle_output_invert;

View File

@@ -114,6 +114,9 @@ namespace Spindles {
// _pwm_gradient = (_pwm_max_value - _pwm_min_value) / (_max_rpm - _min_rpm);
_pwm_chan_num = 0; // Channel 0 is reserved for spindle use
_spinup_delay = spindle_delay_spinup->get() * 1000.0;
_spindown_delay = spindle_delay_spindown->get() * 1000.0;
}
uint32_t PWM::set_rpm(uint32_t rpm) {
@@ -163,14 +166,14 @@ namespace Spindles {
sys.spindle_speed = 0;
stop();
if (use_delays && (_current_state != state)) {
mc_dwell(spindle_delay_spindown->get());
delay(_spinup_delay);
}
} else {
set_dir_pin(state == SpindleState::Cw);
set_rpm(rpm);
set_enable_pin(state != SpindleState::Disable); // must be done after setting rpm for enable features to work
if (use_delays && (_current_state != state)) {
mc_dwell(spindle_delay_spinup->get());
delay(_spindown_delay);
}
}

View File

@@ -73,6 +73,8 @@ namespace Spindles {
bool is_reversable;
bool use_delays; // will SpinUp and SpinDown delays be used.
volatile SpindleState _current_state = SpindleState::Disable;
uint32_t _spinup_delay;
uint32_t _spindown_delay;
static void select();
};

View File

@@ -326,6 +326,9 @@ namespace Spindles {
_min_rpm = rpm_min->get();
_max_rpm = rpm_max->get();
_spinup_delay = spindle_delay_spinup->get() * 1000.0;
_spindown_delay = spindle_delay_spindown->get() * 1000.0;
return pins_settings_ok;
}
@@ -348,16 +351,18 @@ namespace Spindles {
if (_current_state != state) { // already at the desired state. This function gets called a lot.
set_mode(state, critical); // critical if we are in a job
set_rpm(rpm);
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin1");
if (state == SpindleState::Disable) {
sys.spindle_speed = 0;
if (_current_state != state) {
mc_dwell(spindle_delay_spindown->get());
}
delay(_spindown_delay);
} else {
if (_current_state != state) {
mc_dwell(spindle_delay_spinup->get());
}
delay(_spinup_delay);
}
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin2");
} else {
if (_current_rpm != rpm) {
set_rpm(rpm);

View File

@@ -280,7 +280,14 @@ namespace WebUI {
if ((path.substring(0, 4) == "/SD/")) {
//remove /SD
path = path.substring(3);
if (SDState::Idle != get_sd_state(true)) {
String content = "cannot open: ";
content += path + ", SD is not available.";
_webserver->send(500, "text/plain", content);
}
if (SD.exists(pathWithGz) || SD.exists(path)) {
set_sd_state(SDState::BusyUploading);
if (SD.exists(pathWithGz)) {
path = pathWithGz;
}
@@ -311,8 +318,10 @@ namespace WebUI {
if (i != totalFileSize) {
//error: TBD
}
set_sd_state(SDState::Idle);
return;
}
set_sd_state(SDState::Idle);
}
String content = "cannot find ";
content += path;