From f35406490f425baf553c726a36872e9d359b7d13 Mon Sep 17 00:00:00 2001 From: bdring Date: Sat, 14 Nov 2020 08:59:56 -0600 Subject: [PATCH 1/2] Devt (#669) * 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 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 --- Grbl_Esp32/Custom/CoreXY.cpp | 307 ++++++++++++++++++ Grbl_Esp32/Custom/atari_1020.cpp | 2 +- Grbl_Esp32/Custom/custom_code_template.cpp | 8 +- .../Custom/esp32_printer_controller.cpp | 4 +- Grbl_Esp32/Custom/parallel_delta.cpp | 213 +++++++++--- Grbl_Esp32/src/Config.h | 9 +- Grbl_Esp32/src/Defaults.h | 28 ++ Grbl_Esp32/src/Error.cpp | 2 +- Grbl_Esp32/src/Error.h | 2 +- Grbl_Esp32/src/Exec.cpp | 15 + Grbl_Esp32/src/Exec.h | 57 ++++ Grbl_Esp32/src/GCode.cpp | 4 +- Grbl_Esp32/src/Grbl.h | 13 +- Grbl_Esp32/src/Jog.cpp | 7 +- Grbl_Esp32/src/Limits.cpp | 55 +--- Grbl_Esp32/src/Machines/3axis_v3.h | 57 ---- Grbl_Esp32/src/Machines/3axis_xyx.h | 59 ---- .../Machines/6_pack_Lowrider_stepstick_v1.h | 16 +- .../src/Machines/6_pack_MPCNC_stepstick_v1.h | 11 +- .../src/Machines/6_pack_stepstick_XYZ_v1.h | 7 +- Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h | 13 +- Grbl_Esp32/src/Machines/espduino.h | 61 ---- Grbl_Esp32/src/Machines/midtbot.h | 34 +- .../src/Machines/spi_daisy_4axis_xyyz.h | 95 ------ .../src/Machines/{tapster3.h => tapster_3.h} | 51 +-- .../src/Machines/tapster_pro_6P_trinamic.h | 197 +++++++++++ Grbl_Esp32/src/Machines/template.h | 2 +- Grbl_Esp32/src/Machines/test_drive.h | 2 + Grbl_Esp32/src/Machines/tmc2130_pen.h | 8 +- Grbl_Esp32/src/MotionControl.cpp | 14 +- Grbl_Esp32/src/Motors/RcServo.cpp | 2 - Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 101 ++++-- Grbl_Esp32/src/Motors/TrinamicDriver.h | 21 +- Grbl_Esp32/src/NutsBolts.cpp | 6 + Grbl_Esp32/src/NutsBolts.h | 7 +- Grbl_Esp32/src/Planner.cpp | 47 +-- Grbl_Esp32/src/ProcessSettings.cpp | 63 +++- Grbl_Esp32/src/Report.cpp | 14 +- Grbl_Esp32/src/Settings.cpp | 4 +- Grbl_Esp32/src/Settings.h | 25 +- Grbl_Esp32/src/SettingsDefinitions.cpp | 28 +- Grbl_Esp32/src/SettingsDefinitions.h | 2 + Grbl_Esp32/src/System.cpp | 21 +- Grbl_Esp32/src/System.h | 56 +--- Grbl_Esp32/src/WebUI/WebSettings.cpp | 10 +- 45 files changed, 1098 insertions(+), 662 deletions(-) create mode 100644 Grbl_Esp32/Custom/CoreXY.cpp create mode 100644 Grbl_Esp32/src/Exec.cpp create mode 100644 Grbl_Esp32/src/Exec.h delete mode 100644 Grbl_Esp32/src/Machines/3axis_v3.h delete mode 100644 Grbl_Esp32/src/Machines/3axis_xyx.h delete mode 100644 Grbl_Esp32/src/Machines/espduino.h delete mode 100644 Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h rename Grbl_Esp32/src/Machines/{tapster3.h => tapster_3.h} (66%) create mode 100644 Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp new file mode 100644 index 00000000..cefab842 --- /dev/null +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -0,0 +1,307 @@ +/* + CoreXY.cpp - + + Copyright (c) 2020 Barton Dring @buildlog + + https://corexy.com/theory.html + + Limitations + - Must home via $H. $HX type homes not allowed + - Must home one axis per cycle + - limited to 3 axis systems...easy fix in increase (just donate) + + ============================================================================ + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . + FYI: http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ + Better: http://hypertriangle.com/~alex/delta-robot-tutorial/ +*/ +#include "../src/Settings.h" + +// Homing axis search distance multiplier. Computed by this value times the cycle travel. +#ifndef HOMING_AXIS_SEARCH_SCALAR +# define HOMING_AXIS_SEARCH_SCALAR 1.1 // Must be > 1 to ensure limit switch will be engaged. +#endif +#ifndef HOMING_AXIS_LOCATE_SCALAR +# define HOMING_AXIS_LOCATE_SCALAR 2.0 // Must be > 1 to ensure limit switch is cleared. +#endif + +// The midTbot has a quirk where the x motor has to move twice as far as it would +// on a normal T-Bot or CoreXY +#ifndef MIDTBOT +const float geometry_factor = 1.0; +#else +const float geometry_factor = 2.0; +#endif + +static float last_motors[MAX_N_AXIS] = { 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs +static float last_cartesian[MAX_N_AXIS] = {}; + +// prototypes for helper functions +float three_axis_dist(float* point1, float* point2); + +void machine_init() { + // print a startup message to show the kinematics are enable + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY Kinematics Init"); +} + +// This will always return true to prevent the normal Grbl homing cycle +bool user_defined_homing(uint8_t cycle_mask) { + uint8_t n_cycle; // each home is a multi cycle operation approach, pulloff, approach..... + float target[MAX_N_AXIS] = { 0.0 }; // The target for each move in the cycle + float max_travel; + uint8_t axis; + + // check for single axis homing + if (cycle_mask != 0) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY Single axis homing not allowed. Use $H only"); + return true; + } + + // check for multi axis homing per cycle ($Homing/Cycle0=XY type)...not allowed in CoreXY + bool setting_error = false; + for (int cycle = 0; cycle < 3; cycle++) { + if (numberOfSetBits(homing_cycle[cycle]->get()) > 1) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "CoreXY Multi axis homing cycles not allowed. $Homing/Cycle%d=%s", + cycle, + homing_cycle[cycle]->getStringValue()); + setting_error = true; + } + } + if (setting_error) + return true; + + // setup the motion parameters + plan_line_data_t plan_data; + plan_line_data_t* pl_data = &plan_data; + memset(pl_data, 0, sizeof(plan_line_data_t)); + pl_data->motion = {}; + pl_data->motion.systemMotion = 1; + pl_data->motion.noFeedOverride = 1; + + for (int cycle = 0; cycle < 3; cycle++) { + AxisMask mask = homing_cycle[cycle]->get(); + 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++) { + if (bit(axis) == mask) { + // setup for the homing of this axis + bool approach = true; + float homing_rate = homing_seek_rate->get(); + max_travel = HOMING_AXIS_SEARCH_SCALAR * axis_settings[axis]->max_travel->get(); + sys.homing_axis_lock = 0xFF; // we don't need to lock any motors in CoreXY + n_cycle = (2 * NHomingLocateCycle + 1); // approach + ((pulloff + approach) * Cycles) + + do { + bool switch_touched = false; + + // zero all X&Y posiitons before each cycle + for (int idx = X_AXIS; idx <= Y_AXIS; idx++) { + sys_position[idx] = 0.0; + target[idx] = 0.0; + } + + if (bit_istrue(homing_dir_mask->get(), bit(axis))) { + approach ? target[axis] = -max_travel : target[axis] = max_travel; + } else { + approach ? target[axis] = max_travel : target[axis] = -max_travel; + } + + target[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); + + // convert back to motor steps + inverse_kinematics(target); + + pl_data->feed_rate = homing_rate; // feed or seek rates + plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion. + sys.step_control = {}; + sys.step_control.executeSysMotion = true; // Set to execute homing motion and clear existing flags. + st_prep_buffer(); // Prep and fill segment buffer from newly planned block. + st_wake_up(); // Initiate motion + + do { + if (approach) { + switch_touched = bitnum_istrue(limits_get_state(), axis); + } + st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us. + // Exit routines: No time to run protocol_execute_realtime() in this loop. + if (sys_rt_exec_state.bit.safetyDoor || sys_rt_exec_state.bit.reset || cycle_stop) { + ExecState rt_exec_state; + rt_exec_state.value = sys_rt_exec_state.value; + // Homing failure condition: Reset issued during cycle. + if (rt_exec_state.bit.reset) { + sys_rt_exec_alarm = ExecAlarm::HomingFailReset; + } + // Homing failure condition: Safety door was opened. + if (rt_exec_state.bit.safetyDoor) { + sys_rt_exec_alarm = ExecAlarm::HomingFailDoor; + } + // Homing failure condition: Limit switch still engaged after pull-off motion + if (!approach && (limits_get_state() & cycle_mask)) { + sys_rt_exec_alarm = ExecAlarm::HomingFailPulloff; + } + // Homing failure condition: Limit switch not found during approach. + if (approach && cycle_stop) { + sys_rt_exec_alarm = ExecAlarm::HomingFailApproach; + } + + if (sys_rt_exec_alarm != ExecAlarm::None) { + motors_set_homing_mode(cycle_mask, false); // tell motors homing is done...failed + mc_reset(); // Stop motors, if they are running. + protocol_execute_realtime(); + return true; + } else { + // Pull-off motion complete. Disable CYCLE_STOP from executing. + cycle_stop = false; + break; + } + } + } while (!switch_touched); + +#ifdef USE_I2S_STEPS + if (current_stepper == ST_I2S_STREAM) { + if (!approach) { + delay_ms(I2S_OUT_DELAY_MS); + } + } +#endif + st_reset(); // Immediately force kill steppers and reset step segment buffer. + delay_ms(homing_debounce->get()); // Delay to allow transient dynamics to dissipate. + + approach = !approach; + // After first cycle, homing enters locating phase. Shorten search to pull-off distance. + if (approach) { + max_travel = homing_pulloff->get() * HOMING_AXIS_LOCATE_SCALAR; + homing_rate = homing_feed_rate->get(); + } else { + max_travel = homing_pulloff->get(); + homing_rate = homing_seek_rate->get(); + } + } while (n_cycle-- > 0); + } + } + } + + // after sussefully setting X & Y axes, we set the current positions + + // set the cartesian axis position + for (axis = X_AXIS; axis <= Y_AXIS; axis++) { + if (bitnum_istrue(homing_dir_mask->get(), axis)) { + target[axis] = limitsMinPosition(axis) + homing_pulloff->get(); + } else { + target[axis] = limitsMaxPosition(axis) - homing_pulloff->get(); + } + } + + 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); + + // convert to motors + inverse_kinematics(target); + // convert to steps + for (axis = X_AXIS; axis <= Y_AXIS; axis++) { + sys_position[axis] = target[axis] * axis_settings[axis]->steps_per_mm->get(); + } + + sys.step_control = {}; // Return step control to normal operation. + + gc_sync_position(); + plan_sync_position(); + kinematics_post_homing(); + limits_init(); + + return true; +} + +// This function is used by Grbl convert Cartesian to motors +// this does not do any motion control +void inverse_kinematics(float* position) { + float motors[3]; + + 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]; +} + +// Inverse Kinematics calculates motor positions from real world cartesian positions +// position is the current position +// Breaking into segments is not needed with CoreXY, because it is a linear system. +void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos +{ + float dx, dy, dz; // distances in each cartesian axis + float motors[MAX_N_AXIS]; + + float feed_rate = pl_data->feed_rate; // save original feed rate + + // calculate cartesian move distance for each axis + dx = target[X_AXIS] - position[X_AXIS]; + dy = target[Y_AXIS] - position[Y_AXIS]; + dz = target[Z_AXIS] - position[Z_AXIS]; + float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz)); + + motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS]; + motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_AXIS]; + motors[Z_AXIS] = target[Z_AXIS]; + + float motor_distance = three_axis_dist(motors, last_motors); + + if (!pl_data->motion.rapidMotion) { + pl_data->feed_rate *= (motor_distance / dist); + } + + memcpy(last_motors, motors, sizeof(motors)); + + mc_line(motors, pl_data); +} + +// motors -> cartesian +void forward_kinematics(float* position) { + float calc_fwd[MAX_N_AXIS]; + + // 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]); + + position[X_AXIS] = calc_fwd[X_AXIS]; + position[Y_AXIS] = calc_fwd[Y_AXIS]; +} + +bool kinematics_pre_homing(uint8_t cycle_mask) { + return false; +} + +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]; +} + +// this is used used by Grbl soft limits to see if the range of the machine is exceeded. +uint8_t kinematic_limits_check(float* target) { + return true; +} + +void user_m30() {} + +// ================ Local Helper functions ================= + +// Determine the unit distance between (2) 3D points +float three_axis_dist(float* point1, float* point2) { + return sqrt(((point1[0] - point2[0]) * (point1[0] - point2[0])) + ((point1[1] - point2[1]) * (point1[1] - point2[1])) + + ((point1[2] - point2[2]) * (point1[2] - point2[2]))); +} diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp index 278e052a..5661ae6d 100644 --- a/Grbl_Esp32/Custom/atari_1020.cpp +++ b/Grbl_Esp32/Custom/atari_1020.cpp @@ -90,7 +90,7 @@ void solenoidSyncTask(void* pvParameters) { // continue with regular homing after setup // return true if this completes homing -bool user_defined_homing() { +bool user_defined_homing(uint8_t cycle_mask) { // create and start a task to do the special homing homing_phase = HOMING_PHASE_FULL_APPROACH; atari_homing = true; diff --git a/Grbl_Esp32/Custom/custom_code_template.cpp b/Grbl_Esp32/Custom/custom_code_template.cpp index 7a1e962d..bd468f6d 100644 --- a/Grbl_Esp32/Custom/custom_code_template.cpp +++ b/Grbl_Esp32/Custom/custom_code_template.cpp @@ -56,13 +56,13 @@ void machine_init() {} #ifdef USE_CUSTOM_HOMING /* - user_defined_homing() is called at the begining of the normal Grbl_ESP32 homing - sequence. If user_defined_homing() returns false, the rest of normal Grbl_ESP32 + user_defined_homing(uint8_t cycle_mask) is called at the begining of the normal Grbl_ESP32 homing + sequence. If user_defined_homing(uint8_t cycle_mask) returns false, the rest of normal Grbl_ESP32 homing is skipped if it returns false, other normal homing continues. For example, if you need to manually prep the machine for homing, you could implement - user_defined_homing() to wait for some button to be pressed, then return true. + user_defined_homing(uint8_t cycle_mask) to wait for some button to be pressed, then return true. */ -bool user_defined_homing() { +bool user_defined_homing(uint8_t cycle_mask) { // True = done with homing, false = continue with normal Grbl_ESP32 homing return true; } diff --git a/Grbl_Esp32/Custom/esp32_printer_controller.cpp b/Grbl_Esp32/Custom/esp32_printer_controller.cpp index cc356075..6c2a8cd1 100644 --- a/Grbl_Esp32/Custom/esp32_printer_controller.cpp +++ b/Grbl_Esp32/Custom/esp32_printer_controller.cpp @@ -82,13 +82,13 @@ void machine_init() { #ifdef USE_CUSTOM_HOMING /* - user_defined_homing() is called at the begining of the normal Grbl_ESP32 homing + (user_defined_homing) is called at the begining of the normal Grbl_ESP32 homing sequence. If user_defined_homing() returns false, the rest of normal Grbl_ESP32 homing is skipped if it returns false, other normal homing continues. For example, if you need to manually prep the machine for homing, you could implement user_defined_homing() to wait for some button to be pressed, then return true. */ -bool user_defined_homing() { +bool user_defined_homing(uint8_t cycle_mask) { // True = done with homing, false = continue with normal Grbl_ESP32 homing return true; } diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp index b49f77e1..a82de59b 100644 --- a/Grbl_Esp32/Custom/parallel_delta.cpp +++ b/Grbl_Esp32/Custom/parallel_delta.cpp @@ -6,6 +6,9 @@ Jason Huggins, Tapster Robotics Kinematics for a parallel delta robot. + + Note: You must do a clean before compiling whenever this file is altered! + ==================== How it Works ==================================== @@ -24,16 +27,17 @@ The arm 0 values (angle) are the arms at horizontal. Positive angles are below horizontal. The machine's Z zero point in the kinematics is parallel to the arm axes. - Delta_z_offset is the offset to the end effector joints at arm angle zero. - The is calculated at startup and used in the forward kinematics + The offset of the Z distance from the arm axes to the end effector joints + at arm angle zero will be printed at startup on the serial port. - Feedrate in gcode is in the cartesian uints. This must be converted to the + Feedrate in gcode is in the cartesian units. This must be converted to the angles. This is done by calculating the segment move distance and the angle move distance and applying that ration to the feedrate. TODO Cleanup Update so extra axes get delt with ... passed through properly - + Have MPos use kinematics too + ============================================================================ Grbl is free software: you can redistribute it and/or modify @@ -50,13 +54,23 @@ Better: http://hypertriangle.com/~alex/delta-robot-tutorial/ */ +#include "../src/Settings.h" + enum class KinematicError : uint8_t { NONE = 0, OUT_OF_RANGE = 1, ANGLE_TOO_NEGATIVE = 2, + ANGLE_TOO_POSITIVE = 3, }; -// trigonometric constants to speed calculations +// Create custom run time $ settings +FloatSetting* kinematic_segment_len; +FloatSetting* delta_crank_len; +FloatSetting* delta_link_len; +FloatSetting* delta_crank_side_len; +FloatSetting* delta_effector_side_len; + +// trigonometric constants to speed up calculations const float sqrt3 = 1.732050807; const float dtr = M_PI / (float)180.0; // degrees to radians const float sin120 = sqrt3 / 2.0; @@ -66,60 +80,98 @@ const float sin30 = 0.5; const float tan30 = 1.0 / sqrt3; // the geometry of the delta -const float rf = RADIUS_FIXED; // radius of the fixed side (length of motor cranks) -const float re = RADIUS_EFF; // radius of end effector side (length of linkages) -const float f = LENGTH_FIXED_SIDE; // sized of fixed side triangel -const float e = LENGTH_EFF_SIDE; // size of end effector side triangle +float rf; // radius of the fixed side (length of motor cranks) +float re; // radius of end effector side (length of linkages) +float f; // sized of fixed side triangel +float e; // size of end effector side triangle -static float last_angle[N_AXIS] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs -float delta_z_offset; // Z offset of the effector from the arm centers +static float last_angle[3] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs +static float last_cartesian[N_AXIS] = { + 0.0, 0.0, 0.0 +}; // A place to save the previous motor angles for distance/feed rate calcs // Z offset of the effector from the arm centers // prototypes for helper functions int calc_forward_kinematics(float* angles, float* cartesian); KinematicError delta_calcInverse(float* cartesian, float* angles); KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta); float three_axis_dist(float* point1, float* point2); +void read_settings(); void machine_init() { - // Calculate the Z offset at the motor zero angles ... - // Z offset is the z distance from the motor axes to the end effector axes at zero angle float angles[N_AXIS] = { 0.0, 0.0, 0.0 }; float cartesian[N_AXIS] = { 0.0, 0.0, 0.0 }; + // Custom $ settings + kinematic_segment_len = new FloatSetting(EXTENDED, WG, NULL, "Kinematics/SegmentLength", KINEMATIC_SEGMENT_LENGTH, 0.2, 1000.0); + delta_crank_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/CrankLength", RADIUS_FIXED, 50.0, 500.0); + delta_link_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/LinkLength", RADIUS_EFF, 50.0, 500.0); + delta_crank_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/CrankSideLength", LENGTH_FIXED_SIDE, 20.0, 500.0); + delta_effector_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/EffectorSideLength", LENGTH_EFF_SIDE, 20.0, 500.0); + + read_settings(); + + // Calculate the Z offset at the arm zero angles ... + // Z offset is the z distance from the motor axes to the end effector axes at zero angle calc_forward_kinematics(angles, cartesian); // Sets the cartesian values + // print a startup message to show the kinematics are enabled. Print the offset for reference + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]); - // print a startup message to show the kinematics are enables - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delata Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Angle Range %3.3f, %3.3f", MAX_NEGATIVE_ANGLE, MAX_POSITIVE_ANGLE); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "DXL_COUNT_MIN %4.0f CENTER %d MAX %4.0f PER_RAD %d", + // DXL_COUNT_MIN, + // DXL_CENTER, + // DXL_COUNT_MAX, + // DXL_COUNT_PER_RADIAN); } - -bool user_defined_homing() { // true = do not continue with normal Grbl homing +bool user_defined_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing +#ifdef USE_CUSTOM_HOMING return true; +#else + return false; +#endif } +// This function is used by Grbl +void inverse_kinematics(float* position) { + float motor_angles[3]; + + read_settings(); + delta_calcInverse(position, motor_angles); + position[0] = motor_angles[0]; + position[1] = motor_angles[1]; + position[2] = motor_angles[2]; +} + +// This function is used by Grbl void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos { float dx, dy, dz; // distances in each cartesian axis - float motor_angles[N_AXIS]; + float motor_angles[3]; - float seg_target[N_AXIS]; // The target of the current segment - float feed_rate = pl_data->feed_rate; // save original feed rate - bool start_position_erorr = false; - bool show_error = true; // shows error once + float seg_target[3]; // The target of the current segment + float feed_rate = pl_data->feed_rate; // save original feed rate + bool show_error = true; // shows error once - // see if start is in work area...if not skip segments and try to go to target - KinematicError status = delta_calcInverse(position, motor_angles); + KinematicError status; + read_settings(); + + // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start %3.3f %3.3f %3.3f", position[0], position[1], position[2]); + // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target %3.3f %3.3f %3.3f", target[0], target[1], target[2]); + + status = delta_calcInverse(position, motor_angles); if (status == KinematicError::OUT_OF_RANGE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error"); - start_position_erorr = true; + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error %3.3f %3.3f %3.3f", position[0], position[1], position[2]); + //start_position_error = true; } // Check the destination to see if it is in work area status = delta_calcInverse(target, motor_angles); - if (status == KinematicError::OUT_OF_RANGE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable"); - return; + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable error %3.3f %3.3f %3.3f", target[0], target[1], target[2]); } position[X_AXIS] += gc_state.coord_offset[X_AXIS]; @@ -127,14 +179,13 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio position[Z_AXIS] += gc_state.coord_offset[Z_AXIS]; // calculate cartesian move distance for each axis - dx = target[X_AXIS] - position[X_AXIS]; dy = target[Y_AXIS] - position[Y_AXIS]; dz = target[Z_AXIS] - position[Z_AXIS]; float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz)); // determine the number of segments we need ... round up so there is at least 1 (except when dist is 0) - uint32_t segment_count = ceil(dist / SEGMENT_LENGTH); + uint32_t segment_count = ceil(dist / kinematic_segment_len->get()); float segment_dist = dist / ((float)segment_count); // distance of each segment...will be used for feedrate conversion @@ -163,19 +214,46 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio } else { if (show_error) { - grbl_msg_sendf(CLIENT_SERIAL, - MsgLevel::Info, - "Error:%d, Angs X:%4.3f Y:%4.3f Z:%4.3f]\r\n\r\n", - status, - motor_angles[0], - motor_angles[1], - motor_angles[2]); + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "Error:%d, Angs X:%4.3f Y:%4.3f Z:%4.3f", + // status, + // motor_angles[0], + // motor_angles[1], + // motor_angles[2]); show_error = false; } } } } +// this is used used by Grbl soft limits to see if the range of the machine is exceeded. +uint8_t kinematic_limits_check(float* target) { + float motor_angles[3]; + + read_settings(); + + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin Soft Check %3.3f, %3.3f, %3.3f", target[0], target[1], target[2]); + + KinematicError status = delta_calcInverse(target, motor_angles); + + switch (status) { + case KinematicError::OUT_OF_RANGE: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target out of range"); + break; + case KinematicError::ANGLE_TOO_NEGATIVE: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max negative"); + break; + case KinematicError::ANGLE_TOO_POSITIVE: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max positive"); + break; + case KinematicError::NONE: + break; + } + + return (status == KinematicError::NONE); +} + // inverse kinematics: cartesian -> angles // returned status: 0=OK, -1=non-existing position KinematicError delta_calcInverse(float* cartesian, float* angles) { @@ -271,6 +349,10 @@ KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta) { return KinematicError::ANGLE_TOO_NEGATIVE; } + if (theta > MAX_POSITIVE_ANGLE) { + return KinematicError::ANGLE_TOO_POSITIVE; + } + return KinematicError::NONE; } @@ -284,12 +366,17 @@ void forward_kinematics(float* position) { float calc_fwd[N_AXIS]; int status; + read_settings(); + // convert the system position in steps to radians float position_radians[N_AXIS]; int32_t position_steps[N_AXIS]; // Copy current state of the system position variable memcpy(position_steps, sys_position, sizeof(sys_position)); system_convert_array_steps_to_mpos(position_radians, position_steps); + // grbl_msg_sendf( + // CLIENT_SERIAL, MsgLevel::Info, "Fwd Kin Angs %1.3f, %1.3f, %1.3f ", position_radians[0], position_radians[1], position_radians[2]); + // detmine the position of the end effector joint center. status = calc_forward_kinematics(position_radians, calc_fwd); @@ -303,10 +390,56 @@ void forward_kinematics(float* position) { } } -bool kinematics_pre_homing(uint8_t cycle_mask) { +bool kinematics_pre_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing +#ifdef USE_CUSTOM_HOMING return true; +#else + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "kinematics_pre_homing"); + return false; +#endif } -void kinematics_post_homing() {} +void kinematics_post_homing() { +#ifdef USE_CUSTOM_HOMING + +#else + last_angle[X_AXIS] = sys_position[X_AXIS] / axis_settings[X_AXIS]->steps_per_mm->get(); + last_angle[Y_AXIS] = sys_position[Y_AXIS] / axis_settings[Y_AXIS]->steps_per_mm->get(); + last_angle[Z_AXIS] = sys_position[Z_AXIS] / axis_settings[Z_AXIS]->steps_per_mm->get(); + + read_settings(); + + calc_forward_kinematics(last_angle, last_cartesian); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "kinematics_post_homing Angles: %3.3f, %3.3f, %3.3f", + // last_angle[X_AXIS], + // last_angle[Y_AXIS], + // last_angle[Z_AXIS]); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "kinematics_post_homing Cartesian: %3.3f, %3.3f, %3.3f", + // last_cartesian[X_AXIS], + // last_cartesian[Y_AXIS], + // last_cartesian[Z_AXIS]); + + 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]; + +#endif +#ifdef USE_POST_HOMING_DELAY + delay(1000); // give time for servo type homing +#endif +} void user_m30() {} + +void read_settings() { + rf = delta_crank_len->get(); // radius of the fixed side (length of motor cranks) + re = delta_link_len->get(); // radius of end effector side (length of linkages) + f = delta_crank_side_len->get(); // sized of fixed side triangel + e = delta_effector_side_len->get(); // size of end effector side triangle +} diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index 658cade3..50b014f8 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -263,13 +263,6 @@ static const uint8_t NHomingLocateCycle = 1; // Integer (1-128) const double SAFETY_DOOR_SPINDLE_DELAY = 4.0; // Float (seconds) const double SAFETY_DOOR_COOLANT_DELAY = 1.0; // Float (seconds) -// Enable CoreXY kinematics. Use ONLY with CoreXY machines. -// NOTE: This configuration option alters the motion of the X and Y axes to principle of operation -// defined at (http://corexy.com/theory.html). Motors are assumed to positioned and wired exactly as -// described, if not, motions may move in strange directions. Grbl requires the CoreXY A and B motors -// have the same steps per mm internally. -// #define COREXY // Default disabled. Uncomment to enable. - // 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 @@ -539,7 +532,7 @@ const int DEBOUNCE_PERIOD = 32; // in milliseconds default 32 microseconds // these commands may be undesirable. Simply comment the desired macro to disable it. #define ENABLE_RESTORE_WIPE_ALL // '$RST=*' Default enabled. Comment to disable. #define ENABLE_RESTORE_DEFAULT_SETTINGS // '$RST=$' Default enabled. Comment to disable. -#define ENABLE_RESTORE_PARAMETERS // '$RST=#' Default enabled. Comment to disable. +#define ENABLE_RESTORE_CLEAR_PARAMETERS // '$RST=#' Default enabled. Comment to disable. // Additional settings have been added to the original set that you see with the $$ command // Some senders may not be able to parse anything different from the original set diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index f75812a0..6ca3728c 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -70,6 +70,10 @@ # define DEFAULT_STATUS_REPORT_MASK 1 // $10 #endif +#ifndef DEFAULT_VERBOSE_ERRORS +# define DEFAULT_VERBOSE_ERRORS 0 +#endif + #ifndef DEFAULT_JUNCTION_DEVIATION # define DEFAULT_JUNCTION_DEVIATION 0.01 // $11 mm #endif @@ -340,6 +344,30 @@ # define DEFAULT_C_HOMING_MPOS 0.0 #endif +#ifndef DEFAULT_HOMING_CYCLE_0 +# define DEFAULT_HOMING_CYCLE_0 bit(Z_AXIS) +#endif + +#ifndef DEFAULT_HOMING_CYCLE_1 +# define DEFAULT_HOMING_CYCLE_1 (bit(X_AXIS) | bit(Y_AXIS)) +#endif + +#ifndef DEFAULT_HOMING_CYCLE_2 +# define DEFAULT_HOMING_CYCLE_2 0 +#endif + +#ifndef DEFAULT_HOMING_CYCLE_3 +# define DEFAULT_HOMING_CYCLE_3 0 +#endif + +#ifndef DEFAULT_HOMING_CYCLE_4 +# define DEFAULT_HOMING_CYCLE_4 0 +#endif + +#ifndef DEFAULT_HOMING_CYCLE_5 +# define DEFAULT_HOMING_CYCLE_5 0 +#endif + // ========== Motor current (SPI Drivers ) ============= #ifndef DEFAULT_X_CURRENT # define DEFAULT_X_CURRENT 0.25 // $140 current in amps (extended set) diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp index e7fa5de6..149b0665 100644 --- a/Grbl_Esp32/src/Error.cpp +++ b/Grbl_Esp32/src/Error.cpp @@ -21,7 +21,7 @@ #include "Error.h" -std::map ErrorCodes = { +std::map ErrorNames = { { Error::Ok, "No error" }, { Error::ExpectedCommandLetter, "Expected GCodecommand letter" }, { Error::BadNumberFormat, "Bad GCode number format" }, diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h index cfc2b379..09a54030 100644 --- a/Grbl_Esp32/src/Error.h +++ b/Grbl_Esp32/src/Error.h @@ -85,4 +85,4 @@ enum class Error : uint8_t { Eol = 111, }; -extern std::map ErrorCodes; +extern std::map ErrorNames; diff --git a/Grbl_Esp32/src/Exec.cpp b/Grbl_Esp32/src/Exec.cpp new file mode 100644 index 00000000..3cba113d --- /dev/null +++ b/Grbl_Esp32/src/Exec.cpp @@ -0,0 +1,15 @@ +#include "Exec.h" + +std::map AlarmNames = { + { ExecAlarm::None, "None"}, + { ExecAlarm::HardLimit, "Hard Limit"}, + { ExecAlarm::SoftLimit, "Soft Limit"}, + { ExecAlarm::AbortCycle, "Abort Cycle"}, + { ExecAlarm::ProbeFailInitial, "Probe Fail Initial"}, + { ExecAlarm::ProbeFailContact, "Probe Fail Contact"}, + { ExecAlarm::HomingFailReset, "Homing Fail Reset"}, + { ExecAlarm::HomingFailDoor, "Homing Fail Door"}, + { ExecAlarm::HomingFailPulloff, "Homing Fail Pulloff"}, + { ExecAlarm::HomingFailApproach, "Homing Fail Approach"}, + { ExecAlarm::SpindleControl, "Spindle Control"}, +}; diff --git a/Grbl_Esp32/src/Exec.h b/Grbl_Esp32/src/Exec.h new file mode 100644 index 00000000..23d7110d --- /dev/null +++ b/Grbl_Esp32/src/Exec.h @@ -0,0 +1,57 @@ +#pragma once + +#include + +// System executor bit map. Used internally by realtime protocol as realtime command flags, +// which notifies the main program to execute the specified realtime command asynchronously. +// NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default +// flags are always false, so the realtime protocol only needs to check for a non-zero value to +// know when there is a realtime command to execute. +struct ExecStateBits { + uint8_t statusReport : 1; + uint8_t cycleStart : 1; + uint8_t cycleStop : 1; // Unused, per cycle_stop variable + uint8_t feedHold : 1; + uint8_t reset : 1; + uint8_t safetyDoor : 1; + uint8_t motionCancel : 1; + uint8_t sleep : 1; +}; + +union ExecState { + uint8_t value; + ExecStateBits bit; +}; + +static_assert(sizeof(ExecStateBits) == sizeof(uint8_t), "ExecStateBits is not an uint8"); + +// Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides. +// Spindle/coolant and feed/rapids are separated into two controlling flag variables. + +struct ExecAccessoryBits { + uint8_t spindleOvrStop : 1; + uint8_t coolantFloodOvrToggle : 1; + uint8_t coolantMistOvrToggle : 1; +}; + +union ExecAccessory { + uint8_t value; + ExecAccessoryBits bit; +}; + +// Alarm codes. +enum class ExecAlarm : uint8_t { + None = 0, + HardLimit = 1, + SoftLimit = 2, + AbortCycle = 3, + ProbeFailInitial = 4, + ProbeFailContact = 5, + HomingFailReset = 6, + HomingFailDoor = 7, + HomingFailPulloff = 8, + HomingFailApproach = 9, + SpindleControl = 10, +}; + +extern std::map AlarmNames; diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 0134952e..5a24d342 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1473,9 +1473,9 @@ Error gc_execute_line(char* line, uint8_t client) { // and absolute and incremental modes. pl_data->motion.rapidMotion = 1; // Set rapid motion flag. if (axis_command != AxisCommand::None) { - mc_line(gc_block.values.xyz, pl_data); // kinematics kinematics not used for homing righ now + mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); } - mc_line(coord_data, pl_data); + mc_line_kins(coord_data, pl_data, gc_state.position); memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); break; case NonModal::SetHome0: diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index b2e9c2d8..10275f4b 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -23,7 +23,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20201022"; +const char* const GRBL_VERSION_BUILD = "20201101"; //#include #include @@ -96,14 +96,17 @@ void run_once(); void machine_init(); // Called if USE_CUSTOM_HOMING is defined -bool user_defined_homing(); +bool user_defined_homing(uint8_t cycle_mask); // Called if USE_KINEMATICS is defined -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position); -bool kinematics_pre_homing(uint8_t cycle_mask); -void kinematics_post_homing(); + +void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position); +bool kinematics_pre_homing(uint8_t cycle_mask); +void kinematics_post_homing(); +uint8_t kinematic_limits_check(float* target); // Called if USE_FWD_KINEMATICS is defined +void inverse_kinematics(float* position); // used to return a converted value void forward_kinematics(float* position); // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp index 5d7b5773..1c0c3d00 100644 --- a/Grbl_Esp32/src/Jog.cpp +++ b/Grbl_Esp32/src/Jog.cpp @@ -37,8 +37,13 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { return Error::TravelExceeded; } } - // Valid jog command. Plan, set state, and execute. +// Valid jog command. Plan, set state, and execute. +#ifndef USE_KINEMATICS mc_line(gc_block->values.xyz, pl_data); +#else // else use kinematics + inverse_kinematics(gc_block->values.xyz, pl_data, gc_state.position); +#endif + if (sys.state == State::Idle) { if (plan_get_current_block() != NULL) { // Check if there is a block to execute. sys.state = State::Jog; diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 2aed18b7..41198dcf 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -109,11 +109,6 @@ void limits_go_home(uint8_t cycle_mask) { for (uint8_t idx = 0; idx < n_axis; idx++) { // Initialize step pin masks step_pin[idx] = bit(idx); -#ifdef COREXY - if ((idx == A_MOTOR) || (idx == B_MOTOR)) { - step_pin[idx] = (bit(X_AXIS) | bit(Y_AXIS)); - } -#endif if (bit_istrue(cycle_mask, bit(idx))) { // Set target based on max_travel setting. Ensure homing switches engaged with search scalar. max_travel = MAX(max_travel, (HOMING_AXIS_SEARCH_SCALAR)*axis_settings[idx]->max_travel->get()); @@ -133,20 +128,7 @@ void limits_go_home(uint8_t cycle_mask) { // Set target location for active axes and setup computation for homing rate. if (bit_istrue(cycle_mask, bit(idx))) { n_active_axis++; -#ifdef COREXY - if (idx == X_AXIS) { - int32_t axis_position = system_convert_corexy_to_y_axis_steps(sys_position); - sys_position[A_MOTOR] = axis_position; - sys_position[B_MOTOR] = -axis_position; - } else if (idx == Y_AXIS) { - int32_t axis_position = system_convert_corexy_to_x_axis_steps(sys_position); - sys_position[A_MOTOR] = sys_position[B_MOTOR] = axis_position; - } else { - sys_position[Z_AXIS] = 0; - } -#else sys_position[idx] = 0; -#endif // Set target direction based on cycle mask and homing cycle approach state. // NOTE: This happens to compile smaller than any other implementation tried. auto mask = homing_dir_mask->get(); @@ -183,15 +165,7 @@ void limits_go_home(uint8_t cycle_mask) { for (uint8_t idx = 0; idx < n_axis; idx++) { if (axislock & step_pin[idx]) { if (limit_state & bit(idx)) { -#ifdef COREXY - if (idx == Z_AXIS) { - axislock &= ~(step_pin[Z_AXIS]); - } else { - axislock &= ~(step_pin[A_MOTOR] | step_pin[B_MOTOR]); - } -#else axislock &= ~(step_pin[idx]); -#endif } } } @@ -272,33 +246,6 @@ void limits_go_home(uint8_t cycle_mask) { } else { sys_position[idx] = (mpos - pulloff) * steps; } - -#ifdef COREXY - if (idx == X_AXIS) { - int32_t off_axis_position = system_convert_corexy_to_y_axis_steps(sys_position); - sys_position[A_MOTOR] = set_axis_position + off_axis_position; - sys_position[B_MOTOR] = set_axis_position - off_axis_position; - } else if (idx == Y_AXIS) { - int32_t off_axis_position = system_convert_corexy_to_x_axis_steps(sys_position); - sys_position[A_MOTOR] = off_axis_position + set_axis_position; - sys_position[B_MOTOR] = off_axis_position - set_axis_position; - } else { - sys_position[idx] = set_axis_position; - } -#else - //sys_position[idx] = set_axis_position; - /* - float max_mpos, min_mpos; - - if (bit_istrue(homing_dir_mask->get(), bit(idx))) { - min_mpos = mpos; - max_mpos = mpos + travel; - } else { - min_mpos = mpos - travel; - max_mpos = mpos; - } - */ -#endif } } sys.step_control = {}; // Return step control to normal operation. @@ -386,7 +333,7 @@ AxisMask limits_get_state() { return pinMask; } -// Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed, +// Performs a soft limit check. Called from mcline() only. Assumes the machine has been homed, // the workspace volume is in all negative space, and the system is in normal operation. // NOTE: Used by jogging to limit travel within soft-limit volume. void limits_soft_check(float* target) { diff --git a/Grbl_Esp32/src/Machines/3axis_v3.h b/Grbl_Esp32/src/Machines/3axis_v3.h deleted file mode 100644 index 7ed13584..00000000 --- a/Grbl_Esp32/src/Machines/3axis_v3.h +++ /dev/null @@ -1,57 +0,0 @@ -#pragma once -// clang-format off - -/* - 3axis_v3.h - Part of Grbl_ESP32 - - Pin assignments for the ESP32 Development Controller, v3.5. - https://github.com/bdring/Grbl_ESP32_Development_Controller - https://www.tindie.com/products/33366583/grbl_esp32-cnc-development-board-v35/ - - 2018 - Bart Dring - 2020 - Mitch Bradley - - Grbl_ESP32 is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "ESP32_V3.5" - -#define X_STEP_PIN GPIO_NUM_12 -#define X_DIRECTION_PIN GPIO_NUM_26 -#define Y_STEP_PIN GPIO_NUM_14 -#define Y_DIRECTION_PIN GPIO_NUM_25 -#define Z_STEP_PIN GPIO_NUM_27 -#define Z_DIRECTION_PIN GPIO_NUM_33 - -#define X_LIMIT_PIN GPIO_NUM_2 // labeled X Limit -#define Y_LIMIT_PIN GPIO_NUM_4 // labeled Y Limit -#define Z_LIMIT_PIN GPIO_NUM_15 // labeled Z Limit - -// OK to comment out to use pin for other features -#define STEPPERS_DISABLE_PIN GPIO_NUM_13 - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_17 // labeled SpinPWM - -#define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl - -#define COOLANT_MIST_PIN GPIO_NUM_21 // labeled Mist -#define COOLANT_FLOOD_PIN GPIO_NUM_16 // labeled Flood -#define PROBE_PIN GPIO_NUM_32 // labeled Probe - -#define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_35 // labeled Door, needs external pullup -#define CONTROL_RESET_PIN GPIO_NUM_34 // labeled Reset, needs external pullup -#define CONTROL_FEED_HOLD_PIN GPIO_NUM_36 // labeled Hold, needs external pullup -#define CONTROL_CYCLE_START_PIN GPIO_NUM_39 // labeled Start, needs external pullup diff --git a/Grbl_Esp32/src/Machines/3axis_xyx.h b/Grbl_Esp32/src/Machines/3axis_xyx.h deleted file mode 100644 index f7b9bda9..00000000 --- a/Grbl_Esp32/src/Machines/3axis_xyx.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once -// clang-format off - -/* - 3axis_xyx.h - Part of Grbl_ESP32 - - Pin assignments for the ESP32 Development Controller - used to drive a dual motor gantry where the drivers - labeled X, Y and Z drive the machine axes X, Y and X. - https://github.com/bdring/Grbl_ESP32_Development_Controller - https://www.tindie.com/products/33366583/grbl_esp32-cnc-development-board-v35/ - - 2020 - Mitch Bradley - - Grbl_ESP32 is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "ESP32_V4_XYX" -#define X_STEP_PIN GPIO_NUM_26 /* labeled Y */ -#define X_DIRECTION_PIN GPIO_NUM_15 /* labeled Y */ -#define Y_STEP_PIN GPIO_NUM_12 /* labeled X */ -#define Y_DIRECTION_PIN GPIO_NUM_14 /* labeled X */ -#define Y2_STEP_PIN GPIO_NUM_27 /* labeled Z */ -#define Y2_DIRECTION_PIN GPIO_NUM_33 /* labeled Z */ - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_2 -#define SPINDLE_ENABLE_PIN GPIO_NUM_22 - -#define X_LIMIT_PIN GPIO_NUM_17 -#define Y_LIMIT_PIN GPIO_NUM_4 -// #define Z_LIMIT_PIN GPIO_NUM_16 - -#define STEPPERS_DISABLE_PIN GPIO_NUM_13 - -#define COOLANT_MIST_PIN GPIO_NUM_21 -#define COOLANT_FLOOD_PIN GPIO_NUM_25 - - - -// see versions for X and Z -#define PROBE_PIN GPIO_NUM_32 - -#define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_35 // needs external pullup -#define CONTROL_RESET_PIN GPIO_NUM_34 // needs external pullup -#define CONTROL_FEED_HOLD_PIN GPIO_NUM_36 // needs external pullup -#define CONTROL_CYCLE_START_PIN GPIO_NUM_39 // needs external pullup diff --git a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h index 9e935607..8131f6d8 100644 --- a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h @@ -39,37 +39,35 @@ #define I2S_OUT_WS GPIO_NUM_17 #define I2S_OUT_DATA GPIO_NUM_21 -#define X_STEPPER_MS3 I2SO(3) // Labeled X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS -#define X2_STEPPER_MS3 I2SO(14) // A_CS -#define Y2_STEPPER_MS3 I2SO(19) // B_CS - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) +#define X_STEPPER_MS3 I2SO(3) // Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) +#define Y_STEPPER_MS3 I2SO(6) #define Y_DISABLE_PIN I2SO(7) // Motor Socket #3 #define Y2_DISABLE_PIN I2SO(8) #define Y2_DIRECTION_PIN I2SO(9) #define Y2_STEP_PIN I2SO(10) +#define Y2_STEPPER_MS3 I2SO(11) // Motor Socket #4 #define Z_DIRECTION_PIN I2SO(12) #define Z_STEP_PIN I2SO(13) +#define Z_STEPPER_MS3 I2SO(14) #define Z_DISABLE_PIN I2SO(15) // Motor Socket #5 #define Z2_DISABLE_PIN I2SO(16) #define Z2_DIRECTION_PIN I2SO(17) #define Z2_STEP_PIN I2SO(18) - +#define Z2_STEPPER_MS3 I2SO(19) /* Socket I/O reference @@ -112,8 +110,8 @@ Socket #5 // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module #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 +#define Y2_LIMIT_PIN GPIO_NUM_35 +#define Z_LIMIT_PIN GPIO_NUM_34 // 4x Input Module in Socket #2 // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module diff --git a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h index 034dade2..5d3ca518 100644 --- a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h @@ -39,36 +39,35 @@ #define I2S_OUT_WS GPIO_NUM_17 #define I2S_OUT_DATA GPIO_NUM_21 -#define X_STEPPER_MS3 I2SO(3) // Labeled X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS -#define X2_STEPPER_MS3 I2SO(14) // A_CS -#define Y2_STEPPER_MS3 I2SO(19) // B_CS - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) +#define X_STEPPER_MS3 I2SO(3) // Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) +#define Y_STEPPER_MS3 I2SO(6) #define Y_DISABLE_PIN I2SO(7) // Motor Socket #3 #define Z_DISABLE_PIN I2SO(8) #define Z_DIRECTION_PIN I2SO(9) #define Z_STEP_PIN I2SO(10) +#define Z_STEPPER_MS3 I2SO(11) // Motor Socket #4 #define X2_DIRECTION_PIN I2SO(12) #define X2_STEP_PIN I2SO(13) +#define X2_STEPPER_MS3 I2SO(14) #define X2_DISABLE_PIN I2SO(15) // Motor Socket #5 #define Y2_DISABLE_PIN I2SO(16) #define Y2_DIRECTION_PIN I2SO(17) #define Y2_STEP_PIN I2SO(18) +#define Y2_STEPPER_MS3 I2SO(19) /* diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index 28ab79db..b86c0c26 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -41,24 +41,24 @@ #define I2S_OUT_WS GPIO_NUM_17 #define I2S_OUT_DATA GPIO_NUM_21 -#define X_STEPPER_MS3 I2SO(3) // X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) +#define X_STEPPER_MS3 I2SO(3) // Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) +#define Y_STEPPER_MS3 I2SO(6) #define Y_DISABLE_PIN I2SO(7) // Motor Socket #3 #define Z_DISABLE_PIN I2SO(8) #define Z_DIRECTION_PIN I2SO(9) #define Z_STEP_PIN I2SO(10) +#define Z_STEPPER_MS3 I2SO(11) /* Socket I/O reference @@ -124,4 +124,3 @@ Socket #5 #define DEFAULT_X_STEPS_PER_MM 800 #define DEFAULT_Y_STEPS_PER_MM 800 #define DEFAULT_Z_STEPS_PER_MM 800 - diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h index caf673be..084136b0 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h @@ -40,41 +40,40 @@ #define I2S_OUT_DATA GPIO_NUM_21 -#define X_STEPPER_MS3 I2SO(3) // X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS -#define A_STEPPER_MS3 I2SO(14) // A_CS -#define B_STEPPER_MS3 I2SO(19) // B_CS -#define C_STEPPER_MS3 I2SO(22) // C_CS - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) +#define X_STEPPER_MS3 I2SO(3) // Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) +#define Y_STEPPER_MS3 I2SO(6) #define Y_DISABLE_PIN I2SO(7) // Motor Socket #3 #define Z_DISABLE_PIN I2SO(8) #define Z_DIRECTION_PIN I2SO(9) #define Z_STEP_PIN I2SO(10) +#define Z_STEPPER_MS3 I2SO(11) // Motor Socket #4 #define A_DIRECTION_PIN I2SO(12) #define A_STEP_PIN I2SO(13) +#define A_STEPPER_MS3 I2SO(14) #define A_DISABLE_PIN I2SO(15) // Motor Socket #5 #define B_DISABLE_PIN I2SO(16) #define B_DIRECTION_PIN I2SO(17) #define B_STEP_PIN I2SO(18) +#define B_STEPPER_MS3 I2SO(19) // Motor Socket #5 #define C_DIRECTION_PIN I2SO(20) #define C_STEP_PIN I2SO(21) +#define C_STEPPER_MS3 I2SO(22) #define C_DISABLE_PIN I2SO(23) diff --git a/Grbl_Esp32/src/Machines/espduino.h b/Grbl_Esp32/src/Machines/espduino.h deleted file mode 100644 index 7bc6b8a4..00000000 --- a/Grbl_Esp32/src/Machines/espduino.h +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once -// clang-format off - -/* - espduino.h - Part of Grbl_ESP32 - - Pin assignments for ESPDUINO-32 Boards and Protoneer V3 boards - Note: Probe pin is mapped, but will require a 10k external pullup to 3.3V to work. - - Rebooting...See this issue https://github.com/bdring/Grbl_Esp32/issues/314 - !!!! Experimental Untested !!!!! - - 2019 - Bart Dring - 2020 - Mitch Bradley - - Grbl_ESP32 is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "ESPDUINO_32" - -#define X_STEP_PIN GPIO_NUM_26 -#define X_DIRECTION_PIN GPIO_NUM_16 - -#define Y_STEP_PIN GPIO_NUM_25 -#define Y_DIRECTION_PIN GPIO_NUM_27 - -#define Z_STEP_PIN GPIO_NUM_17 -#define Z_DIRECTION_PIN GPIO_NUM_14 - -// OK to comment out to use pin for other features -#define STEPPERS_DISABLE_PIN GPIO_NUM_12 - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_19 - -#define SPINDLE_DIR_PIN GPIO_NUM_18 - -#define COOLANT_FLOOD_PIN GPIO_NUM_34 -#define COOLANT_MIST_PIN GPIO_NUM_36 - -#define X_LIMIT_PIN GPIO_NUM_13 -#define Y_LIMIT_PIN GPIO_NUM_5 -#define Z_LIMIT_PIN GPIO_NUM_19 - -#define PROBE_PIN GPIO_NUM_39 - -#define CONTROL_RESET_PIN GPIO_NUM_2 -#define CONTROL_FEED_HOLD_PIN GPIO_NUM_4 -#define CONTROL_CYCLE_START_PIN GPIO_NUM_35 // ESP32 needs external pullup diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h index b9ceee16..41e35c2a 100644 --- a/Grbl_Esp32/src/Machines/midtbot.h +++ b/Grbl_Esp32/src/Machines/midtbot.h @@ -25,7 +25,15 @@ along with Grbl_ESP32. If not, see . */ -#define MACHINE_NAME "MIDTBOT" +#define MACHINE_NAME "midTbot" + +#define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp" + +#define MIDTBOT // applies the geometry correction to the kinematics +#define USE_KINEMATICS // there are kinematic equations for this machine +#define USE_FWD_KINEMATICS // report in cartesian +#define USE_MACHINE_INIT // There is some custom initialization for this machine +#define USE_CUSTOM_HOMING #define SPINDLE_TYPE SpindleType::NONE @@ -35,10 +43,6 @@ #define X_DIRECTION_PIN GPIO_NUM_26 #define Y_DIRECTION_PIN GPIO_NUM_25 -#ifndef COREXY // maybe set in config.h - #define COREXY -#endif - #define STEPPERS_DISABLE_PIN GPIO_NUM_13 #define X_LIMIT_PIN GPIO_NUM_2 @@ -51,8 +55,11 @@ #define SPINDLE_TYPE SpindleType::NONE // defaults -#define DEFAULT_HOMING_CYCLE_0 bit(Y_AXIS) -#define DEFAULT_HOMING_CYCLE_1 bit(X_AXIS) +#define DEFAULT_HOMING_CYCLE_0 bit(Z_AXIS) +#define DEFAULT_HOMING_CYCLE_1 bit(Y_AXIS) +#define DEFAULT_HOMING_CYCLE_2 bit(X_AXIS) + +#define DEFAULT_HOMING_DIR_MASK (bit(X_AXIS) | bit (Z_AXIS)) // these home negative #define DEFAULT_STEP_PULSE_MICROSECONDS 3 #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on @@ -73,13 +80,12 @@ #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 1 -#define DEFAULT_HOMING_DIR_MASK 1 -#define DEFAULT_HOMING_FEED_RATE 200.0 // mm/min -#define DEFAULT_HOMING_SEEK_RATE 1000.0 // mm/min +#define DEFAULT_HOMING_FEED_RATE 500.0 // mm/min +#define DEFAULT_HOMING_SEEK_RATE 2000.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 3.0 // mm -#define DEFAULT_X_STEPS_PER_MM 200.0 +#define DEFAULT_X_STEPS_PER_MM 100.0 #define DEFAULT_Y_STEPS_PER_MM 100.0 #define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode @@ -93,4 +99,8 @@ #define DEFAULT_X_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value. #define DEFAULT_Y_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value. -#define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode +#define DEFAULT_Z_MAX_TRAVEL 5.0 // This is percent in servo mode + +#define DEFAULT_X_HOMING_MPOS DEFAULT_Z_MAX_TRAVEL // stays up after homing + + diff --git a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h deleted file mode 100644 index 1bd15efd..00000000 --- a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h +++ /dev/null @@ -1,95 +0,0 @@ -#pragma once -// clang-format off - -/* - spi_daisy_4axis_xyyz.h - Part of Grbl_ESP32 - - Pin assignments for a 3-axis with Y ganged using Triaminic drivers - in daisy-chained SPI mode. - https://github.com/bdring/4_Axis_SPI_CNC - - 2019 - Bart Dring - 2020 - Mitch Bradley - - Grbl_ESP32 is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "SPI_DAISY_4X_xyyz" - -#ifdef N_AXIS - #undef N_AXIS -#endif -#define N_AXIS 3 // can be 3 or 4. (if 3 install bypass jumper next to the A driver) - -#define TRINAMIC_DAISY_CHAIN - -#define TRINAMIC_RUN_MODE TrinamicMode :: CoolStep -#define TRINAMIC_HOMING_MODE TrinamicMode :: CoolStep - -// Use SPI enable instead of the enable pin -// The hardware enable pin is tied to ground -#define USE_TRINAMIC_ENABLE - -#define DEFAULT_HOMING_SQUARED_AXES bit(Y_AXIS) - -// Y motor connects to the 1st driver -#define X_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define X_RSENSE TMC2130_RSENSE_DEFAULT -#define X_STEP_PIN GPIO_NUM_12 -#define X_DIRECTION_PIN GPIO_NUM_14 -#define X_CS_PIN GPIO_NUM_17 // Daisy Chain, all share same CS pin - -// Y motor connects to the 2nd driver -#define Y_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define Y_RSENSE TMC2130_RSENSE_DEFAULT -#define Y_STEP_PIN GPIO_NUM_27 -#define Y_DIRECTION_PIN GPIO_NUM_26 -#define Y_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin - -// Y2 motor connects to the 2nd driver -#define Y2_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define Y2_RSENSE TMC2130_RSENSE_DEFAULT -#define Y2_STEP_PIN GPIO_NUM_15 // Z on schem -#define Y2_DIRECTION_PIN GPIO_NUM_2 // Z on schem -#define Y2_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin - -// Z Axis motor connects to the 4th driver -#define Z_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define Z_RSENSE TMC2130_RSENSE_DEFAULT -#define Z_STEP_PIN GPIO_NUM_33 // A on schem -#define Z_DIRECTION_PIN GPIO_NUM_32 // A on schem -#define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin - -// Mist is a 3.3V output -// Turn on with M7 and off with M9 -#define COOLANT_MIST_PIN GPIO_NUM_21 - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_25 -#define SPINDLE_ENABLE_PIN GPIO_NUM_4 - -// Relay operation -// Install Jumper near relay -// For spindle Use max RPM of 1 -// For PWM remove jumper and set MAX RPM to something higher ($30 setting) -// Interlock jumper along top edge needs to be installed for both versions -#define DEFAULT_SPINDLE_RPM_MAX 1 // Should be 1 for relay operation - -#define PROBE_PIN GPIO_NUM_22 - -#define X_LIMIT_PIN GPIO_NUM_36 -#define Y_LIMIT_PIN GPIO_NUM_39 -#define Z_LIMIT_PIN GPIO_NUM_34 - diff --git a/Grbl_Esp32/src/Machines/tapster3.h b/Grbl_Esp32/src/Machines/tapster_3.h similarity index 66% rename from Grbl_Esp32/src/Machines/tapster3.h rename to Grbl_Esp32/src/Machines/tapster_3.h index c29d018d..b4de2b1c 100644 --- a/Grbl_Esp32/src/Machines/tapster3.h +++ b/Grbl_Esp32/src/Machines/tapster_3.h @@ -26,17 +26,24 @@ // ================= Firmware Customization =================== #define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine +#define USE_FWD_KINEMATICS // report in cartesian +#define USE_MACHINE_INIT // There is some custom initialization for this machine // ================== Delta Geometry =========================== -#define RADIUS_FIXED 70.0; // radius of the fixed side (length of motor cranks) -#define RADIUS_EFF 133.5; // radius of end effector side (length of linkages) -#define LENGTH_FIXED_SIDE 179.437f; // sized of fixed side triangel -#define LENGTH_EFF_SIDE 86.6025f; // size of end effector side triangle -#define SEGMENT_LENGTH 0.5 // segment length in mm -#define MAX_NEGATIVE_ANGLE -45 // in negative radians how far can the arms go up before damaging machine (max pi/2) +#define RADIUS_FIXED 70.0 // radius of the fixed side (length of motor cranks) +#define RADIUS_EFF 133.5 // radius of end effector side (length of linkages) +#define LENGTH_FIXED_SIDE 179.437f // sized of fixed side triangel +#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle +#define KINEMATIC_SEGMENT_LENGTH 1.0f // segment length in mm +#define MAX_NEGATIVE_ANGLE -(M_PI / 3.0) // 60 degrees up +#define MAX_POSITIVE_ANGLE (M_PI / 2.0) // 90 degrees down +#define ARM_INTERNAL_ANGLE 0.05 // radians 2.866° // due to mounting angle +#define USE_POST_HOMING_DELAY // Servos need time to reach destination before idle kicks in + +#define DEFAULT_X_MAX_TRAVEL MAX_POSITIVE_ANGLE - MAX_NEGATIVE_ANGLE +#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // +#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // // =================== Machine Hardware Definition ============= @@ -48,9 +55,17 @@ #define Y_DYNAMIXEL_ID 2 // protocol ID #define Z_DYNAMIXEL_ID 3 // protocol ID -// limit servo to 180 degree motion -#define DXL_COUNT_MIN 1024 -#define DXL_COUNT_MAX 3072 +// limit servo to motion range +#define DXL_COUNTS 4096 +#define DXL_COUNT_PER_RADIAN 652 // (DXL_COUNTS / 2.0 * M_PI) + +#define DXL_CENTER 2015 // (DXL_COUNTS / 2) - (ARM_INTERNAL_ANGLE * DXL_COUNT_PER_RADIAN) + +#undef DXL_COUNT_MIN +#define DXL_COUNT_MIN (DXL_CENTER + (MAX_NEGATIVE_ANGLE * DXL_COUNT_PER_RADIAN)) + +#undef DXL_COUNT_MAX +#define DXL_COUNT_MAX (DXL_CENTER + (MAX_POSITIVE_ANGLE * DXL_COUNT_PER_RADIAN)) #define SERVO_TIMER_INTERVAL 50 @@ -84,6 +99,11 @@ #define DEFAULT_HOMING_ENABLE 0 #define DEFAULT_HOMING_DIR_MASK 0 +#define DEFAULT_HOMING_CYCLE_0 (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) +#define DEFAULT_X_HOMING_MPOS MAX_NEGATIVE_ANGLE +#define DEFAULT_Y_HOMING_MPOS DEFAULT_X_HOMING_MPOS +#define DEFAULT_Z_HOMING_MPOS DEFAULT_X_HOMING_MPOS + // the following must have the same values for each axis #define DEFAULT_X_STEPS_PER_MM 800 // value is actually arbitrary, but keep it smallish @@ -98,14 +118,5 @@ #define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION // 10*60*60 mm/min^2 = 10 mm/sec^2 #define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION -#define DEFAULT_X_MAX_TRAVEL 3.14159265 // 180° in radians -#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // -#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // - -#define ARM_INTERNAL_ANGLE 2.866 // due to mounting angle - -#define DEFAULT_X_HOMING_MPOS (DEFAULT_X_MAX_TRAVEL / 2.0) -#define DEFAULT_Y_HOMING_MPOS DEFAULT_X_HOMING_MPOS -#define DEFAULT_Z_HOMING_MPOS DEFAULT_X_HOMING_MPOS #define SPINDLE_TYPE SpindleType::NONE diff --git a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h new file mode 100644 index 00000000..6f88a816 --- /dev/null +++ b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h @@ -0,0 +1,197 @@ +#pragma once +// clang-format off + +/* + tapster_pro_stepstick.h + + 2020 - Bart Dring, Jason Huggins (Tapster Robotics) + + 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 "Tapster Pro Delta 6P Trinamic" +#define CUSTOM_CODE_FILENAME "Custom/parallel_delta.cpp" +/* +// enable these special machine functions to be called from the main program +#define USE_KINEMATICS // there are kinematic equations for this machine +#define FWD_KINEMATICS_REPORTING // report in cartesian +#define USE_RMT_STEPS // Use the RMT periferal to generate step pulses +#define USE_TRINAMIC // some Trinamic motors are used on this machine +#define USE_MACHINE_TRINAMIC_INIT // there is a machine specific setup for the drivers +#define USE_MACHINE_INIT // There is some custom initialization for this machine + +#define SEGMENT_LENGTH 0.5 // segment length in mm +#define KIN_ANGLE_CALC_OK 0 +#define KIN_ANGLE_ERROR -1 + +#define MAX_NEGATIVE_ANGLE -36 // in degrees how far can the arms go up? + +#define HOMING_CURRENT_REDUCTION 1.0 + +*/ + +#define N_AXIS 3 + +#define USE_KINEMATICS // there are kinematic equations for this machine +#define USE_FWD_KINEMATICS // report in cartesian +#define USE_MACHINE_INIT // There is some custom initialization for this machine + +// ================== Delta Geometry =========================== + +#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks) +#define RADIUS_EFF 220.0f // radius of end effector side (length of linkages) +#define LENGTH_FIXED_SIDE 294.449f // sized of fixed side triangel +#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle +#define KINEMATIC_SEGMENT_LENGTH 1.0f // segment length in mm +#define MAX_NEGATIVE_ANGLE -0.75f // +#define MAX_POSITIVE_ANGLE (M_PI / 2.0) // + + +// ================== Config ====================== + +// Set $Homing/Cycle0=XYZ + + + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +#define USE_I2S_STEPS +//#define DEFAULT_STEPPER ST_I2S_STATIC +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE + +// #define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set + +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 + +// ================== CPU MAP ====================== + +// Motor Socket #1 +#define X_TRINAMIC_DRIVER 2130 +#define X_DISABLE_PIN I2SO(0) +#define X_DIRECTION_PIN I2SO(1) +#define X_STEP_PIN I2SO(2) +#define X_CS_PIN I2SO(3) +#define X_RSENSE TMC2130_RSENSE_DEFAULT + +// Motor Socket #2 +#define Y_TRINAMIC_DRIVER X_TRINAMIC_DRIVER +#define Y_DIRECTION_PIN I2SO(4) +#define Y_STEP_PIN I2SO(5) +#define Y_DISABLE_PIN I2SO(7) +#define Y_CS_PIN I2SO(6) +#define Y_RSENSE X_RSENSE + +// Motor Socket #3 +#define Z_TRINAMIC_DRIVER X_TRINAMIC_DRIVER +#define Z_DISABLE_PIN I2SO(8) +#define Z_DIRECTION_PIN I2SO(9) +#define Z_STEP_PIN I2SO(10) +#define Z_CS_PIN I2SO(11) +#define Z_RSENSE X_RSENSE + +// 6 Pack Pin Mapping +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/Socket-Pin-Number-Mapping + +// // 4x Switch input module on CNC I/O module Socket #1 +// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module +#define X_LIMIT_PIN GPIO_NUM_33 +#define Y_LIMIT_PIN GPIO_NUM_32 +#define Z_LIMIT_PIN GPIO_NUM_35 + +// 4x Switch input module on CNC I/O module Socket #2 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module +// #define X_LIMIT_PIN GPIO_NUM_2 +// #define Y_LIMIT_PIN GPIO_NUM_25 +// #define Z_LIMIT_PIN GPIO_NUM_39 +// #define PROBE_PIN GPIO_NUM_36 + +//Example Quad MOSFET module on socket #3 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/Quad-MOSFET-Module +#define USER_DIGITAL_PIN_0 GPIO_NUM_26 +#define USER_DIGITAL_PIN_1 GPIO_NUM_4 +#define USER_DIGITAL_PIN_2 GPIO_NUM_16 +#define USER_DIGITAL_PIN_3 GPIO_NUM_27 + +// Example Servo module in socket #4 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/RC-Servo-BESC-CNC-I-O-Module +// https://github.com/bdring/Grbl_Esp32/wiki/M62,-M63,-M64,-M65-&-M67-User-I-O-Commands +#define USER_ANALOG_PIN_0 GPIO_NUM_14 +#define USER_ANALOG_PIN_1 GPIO_NUM_13 +#define USER_ANALOG_PIN_2 GPIO_NUM_15 +#define USER_ANALOG_PIN_3 GPIO_NUM_12 +#define USER_ANALOG_PIN_0_FREQ 50 // for use with RC servos +#define USER_ANALOG_PIN_1_FREQ 50 +#define USER_ANALOG_PIN_2_FREQ 50 +#define USER_ANALOG_PIN_3_FREQ 50 + +// ================= defaults =========================== + +#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // keep them on, the trinamics will reduce power at idle + +#define DEFAULT_X_MICROSTEPS 8 +#define DEFAULT_Y_MICROSTEPS DEFAULT_X_MICROSTEPS +#define DEFAULT_Z_MICROSTEPS DEFAULT_X_MICROSTEPS + +// some math to figure out microsteps per unit // units could bedegrees or radians +#define UNITS_PER_REV (2.0 * M_PI) // 360.0 degrees or 6.2831853 radians +#define STEPS_PER_REV 400.0 +#define REDUCTION_RATIO (60.0 / 16.0) // the pulleys on arm and motor +#define MICROSTEPS_PER_REV (STEPS_PER_REV * (float)DEFAULT_X_MICROSTEPS * REDUCTION_RATIO) + +#define DEFAULT_X_STEPS_PER_MM (MICROSTEPS_PER_REV / UNITS_PER_REV) +#define DEFAULT_Y_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM +#define DEFAULT_Z_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM + +#define DEFAULT_X_MAX_RATE 100.0 // mm/min +#define DEFAULT_Y_MAX_RATE DEFAULT_X_MAX_RATE +#define DEFAULT_Z_MAX_RATE DEFAULT_X_MAX_RATE + +#define DEFAULT_X_ACCELERATION 20.0 +#define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION +#define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION + +#define DEFAULT_X_CURRENT 1.0 +#define DEFAULT_X_HOLD_CURRENT 0.5 +#define DEFAULT_Y_CURRENT 1.0 +#define DEFAULT_Y_HOLD_CURRENT 0.5 +#define DEFAULT_Z_CURRENT 1.0 +#define DEFAULT_Z_HOLD_CURRENT 0.5 + +// homing +#define DEFAULT_HOMING_FEED_RATE 25 +#define DEFAULT_HOMING_SEEK_RATE 100 +#define DEFAULT_HOMING_DIR_MASK (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) // all axes home negative +#define DEFAULT_HOMING_ENABLE 1 +#define DEFAULT_INVERT_LIMIT_PINS 0 +#define DEFAULT_HOMING_CYCLE_0 (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) +#define DEFAULT_HOMING_CYCLE_1 0 // override this one in defaults.h + +// The machine homes up and above center. MPos is the axis angle in radians +// at the homing posiiton + +#define DEFAULT_X_HOMING_MPOS -0.75 // neagtive because above horizontal +#define DEFAULT_Y_HOMING_MPOS -0.75 +#define DEFAULT_Z_HOMING_MPOS -0.75 + +// the total travel is straight down from horizontal (pi/2) + the up travel +#define DEFAULT_X_MAX_TRAVEL ((M_PI / 2.0) - DEFAULT_X_HOMING_MPOS) +#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL +#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL + +#define DEFAULT_HOMING_PULLOFF -DEFAULT_X_HOMING_MPOS + +#define DEFAULT_INVERT_PROBE_PIN 1 + +#define SPINDLE_TYPE SpindleType::NONE \ No newline at end of file diff --git a/Grbl_Esp32/src/Machines/template.h b/Grbl_Esp32/src/Machines/template.h index 644734da..7672d31f 100644 --- a/Grbl_Esp32/src/Machines/template.h +++ b/Grbl_Esp32/src/Machines/template.h @@ -182,7 +182,7 @@ // functions. custom_code_template.cpp describes the functions // that you can implement. The ifdef guards are described below: // -// USE_CUSTOM_HOMING enables the user_defined_homing() function +// USE_CUSTOM_HOMING enables the user_defined_homing(uint8_t cycle_mask) function // that can implement an arbitrary homing sequence. // #define USE_CUSTOM_HOMING diff --git a/Grbl_Esp32/src/Machines/test_drive.h b/Grbl_Esp32/src/Machines/test_drive.h index d92e6d50..998f40f7 100644 --- a/Grbl_Esp32/src/Machines/test_drive.h +++ b/Grbl_Esp32/src/Machines/test_drive.h @@ -37,6 +37,8 @@ #define MACHINE_NAME "Test Drive - Demo Only No I/O!" + +#define N_AXIS 3 // This cannot use homing because there are no switches #ifdef DEFAULT_HOMING_CYCLE_0 #undef DEFAULT_HOMING_CYCLE_0 diff --git a/Grbl_Esp32/src/Machines/tmc2130_pen.h b/Grbl_Esp32/src/Machines/tmc2130_pen.h index 93380577..24a0a88e 100644 --- a/Grbl_Esp32/src/Machines/tmc2130_pen.h +++ b/Grbl_Esp32/src/Machines/tmc2130_pen.h @@ -59,10 +59,10 @@ // Define one of these 2 options for spindle or servo #define Z_SERVO_PIN GPIO_NUM_27 // comment this out if PWM spindle/laser control. +#define DEFAULT_Z_MAX_TRAVEL 5.0 // Range of travel is 5mm +#define DEFAULT_Z_HOMING_MPOS 5.0 // MPos will be set to 5mm after homing +#define Z_SERVO_CAL_MIN 1.0 // calibration factor for the minimum PWM duty +#define Z_SERVO_CAL_MAX 1.0 // calibration factor for the maximum PWM duty // #define X_LIMIT_PIN See version section at beginning of file #define Y_LIMIT_PIN GPIO_NUM_4 - -// defaults -#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is used as the servo calibration -#define DEFAULT_Z_MAX_TRAVEL 300.0 // This is used as the servo calibration diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index a3be9ce9..5d725ac9 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -115,8 +115,10 @@ void mc_arc(float* target, float r_axis1 = -offset[axis_1]; float rt_axis0 = target[axis_0] - center_axis0; float rt_axis1 = target[axis_1] - center_axis1; + + float previous_position[MAX_N_AXIS] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; #ifdef USE_KINEMATICS - float previous_position[MAX_N_AXIS]; + uint16_t n; auto n_axis = number_axis->get(); for (n = 0; n < n_axis; n++) { @@ -218,11 +220,7 @@ void mc_arc(float* target, } } // Ensure last segment arrives at target location. -#ifdef USE_KINEMATICS mc_line_kins(target, pl_data, previous_position); -#else - mc_line(target, pl_data); -#endif } // Execute dwell in seconds. @@ -285,7 +283,7 @@ static bool axis_is_squared(uint8_t axis_mask) { void mc_homing_cycle(uint8_t cycle_mask) { bool no_cycles_defined = true; #ifdef USE_CUSTOM_HOMING - if (user_defined_homing()) { + if (user_defined_homing(cycle_mask)) { return; } #endif @@ -352,7 +350,6 @@ void mc_homing_cycle(uint8_t cycle_mask) { } } } - if (no_cycles_defined) { report_status_message(Error::HomingNoCycles, CLIENT_ALL); } @@ -411,7 +408,8 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par return GCUpdatePos::None; // Nothing else to do but bail. } // Setup and queue probing motion. Auto cycle-start should not start the cycle. - mc_line(target, pl_data); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Found"); + mc_line_kins(target, pl_data, gc_state.position); // Activate the probing state monitor in the stepper module. sys_probe_state = Probe::Active; // Perform probing cycle. Wait here until probe is triggered or motion completes. diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp index a5716701..4d9a6dd7 100644 --- a/Grbl_Esp32/src/Motors/RcServo.cpp +++ b/Grbl_Esp32/src/Motors/RcServo.cpp @@ -87,8 +87,6 @@ namespace Motors { // Homing justs sets the new system position and the servo will move there bool RcServo::set_homing_mode(bool isHoming) { - float home_pos = 0.0; - sys_position[_axis_index] = axis_settings[_axis_index]->home_mpos->get() * axis_settings[_axis_index]->steps_per_mm->get(); // convert to steps diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index d2c6e595..af0f39e7 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -126,7 +126,7 @@ namespace Motors { void TrinamicDriver::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Trinamic TMC%d Step:%s Dir:%s CS:%s Disable:%s Index:%d %s", + "%s Trinamic TMC%d Step:%s Dir:%s CS:%s Disable:%s Index:%d R:%0.3f %s", reportAxisNameMsg(_axis_index, _dual_axis_index), _driver_part_number, pinName(_step_pin).c_str(), @@ -134,6 +134,7 @@ namespace Motors { pinName(_cs_pin).c_str(), pinName(_disable_pin).c_str(), _spi_index, + _r_sense, reportAxisLimitsMsg(_axis_index)); } @@ -161,24 +162,17 @@ namespace Motors { status.sr = tmcstepper->DRV_STATUS(); bool err = false; - // look for open or short to ground on a and b - if (status.s2ga || status.s2gb) { - grbl_msg_sendf(CLIENT_SERIAL, - MsgLevel::Info, - "%s Motor Short Coil a:%s b:%s", - reportAxisNameMsg(_axis_index, _dual_axis_index), - status.s2ga ? "Y" : "N", - status.s2gb ? "Y" : "N"); + + // look for errors + if (report_short_to_ground(status)) { err = true; } - // check for over temp or pre-warning - if (status.ot || status.otpw) { - grbl_msg_sendf(CLIENT_SERIAL, - MsgLevel::Info, - "%s Driver Temp Warning:%s Fault:%s", - reportAxisNameMsg(_axis_index, _dual_axis_index), - status.otpw ? "Y" : "N", - status.ot ? "Y" : "N"); + + if (report_over_temp(status)) { + err = true; + } + + if (report_short_to_ps(status)) { err = true; } @@ -214,8 +208,6 @@ namespace Motors { if (hold_i_percent > 1.0) hold_i_percent = 1.0; } - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Current run %d hold %f", reportAxisNameMsg(_axis_index, _dual_axis_index), run_i_ma, hold_i_percent); - tmcstepper->microsteps(axis_settings[_axis_index]->microsteps->get()); tmcstepper->rms_current(run_i_ma, hold_i_percent); } @@ -293,6 +285,22 @@ namespace Motors { tmcstepper->sg_result(), feedrate, axis_settings[_axis_index]->stallguard->get()); + + TMC2130_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. + status.sr = tmcstepper->DRV_STATUS(); + + // these only report if there is a fault condition + report_open_load(status); + report_short_to_ground(status); + report_over_temp(status); + report_short_to_ps(status); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "%s Status Register %08x GSTAT %02x", + // reportAxisNameMsg(_axis_index, _dual_axis_index), + // status.sr, + // tmcstepper->GSTAT()); } // calculate a tstep from a rate @@ -366,4 +374,59 @@ namespace Motors { reportTaskStackSize(uxHighWaterMark); } } + + // =========== Reporting functions ======================== + + bool TrinamicDriver::report_open_load(TMC2130_n ::DRV_STATUS_t status) { + if (status.ola || status.olb) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver Open Load a:%s b:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.ola ? "Y" : "N", + status.olb ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicDriver::report_short_to_ground(TMC2130_n ::DRV_STATUS_t status) { + if (status.s2ga || status.s2gb) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver Short Coil a:%s b:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.s2ga ? "Y" : "N", + status.s2gb ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicDriver::report_over_temp(TMC2130_n ::DRV_STATUS_t status) { + if (status.ot || status.otpw) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver Temp Warning:%s Fault:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.otpw ? "Y" : "N", + status.ot ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicDriver::report_short_to_ps(TMC2130_n ::DRV_STATUS_t status) { + // check for short to power supply + if ((status.sr & bit(12)) || (status.sr & bit(13))) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver Short vsa:%s vsb:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + (status.sr & bit(12)) ? "Y" : "N", + (status.sr & bit(13)) ? "Y" : "N"); + return true; + } + return false; // no error + } } diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.h b/Grbl_Esp32/src/Motors/TrinamicDriver.h index 004aa0dd..facedf0d 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.h +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.h @@ -78,9 +78,7 @@ namespace Motors { uint8_t cs_pin, uint16_t driver_part_number, float r_sense) : - TrinamicDriver(axis_index, step_pin, dir_pin, disable_pin, - cs_pin, driver_part_number, r_sense, get_next_index()) - {} + TrinamicDriver(axis_index, step_pin, dir_pin, disable_pin, cs_pin, driver_part_number, r_sense, get_next_index()) {} TrinamicDriver(uint8_t axis_index, uint8_t step_pin, @@ -112,18 +110,23 @@ namespace Motors { bool _disabled; TrinamicMode _mode = TrinamicMode::None; - bool test(); - void set_mode(bool isHoming); - void trinamic_test_response(); - void trinamic_stepper_enable(bool enable); + bool test(); + void set_mode(bool isHoming); + void trinamic_test_response(); + void trinamic_stepper_enable(bool enable); + + bool report_open_load(TMC2130_n ::DRV_STATUS_t status); + bool report_short_to_ground(TMC2130_n ::DRV_STATUS_t status); + bool report_over_temp(TMC2130_n ::DRV_STATUS_t status); + bool report_short_to_ps(TMC2130_n ::DRV_STATUS_t status); uint8_t get_next_index(); // Linked list of Trinamic driver instances, used by the // StallGuard reporting task. static TrinamicDriver* List; - TrinamicDriver* link; - static void readSgTask(void*); + TrinamicDriver* link; + static void readSgTask(void*); protected: void config_message() override; diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp index 591ef5e3..036be861 100644 --- a/Grbl_Esp32/src/NutsBolts.cpp +++ b/Grbl_Esp32/src/NutsBolts.cpp @@ -231,3 +231,9 @@ char* trim(char* str) { end[1] = '\0'; return str; } + +// Returns the number of set number of set bits +// Would return 3 for 01100010 +int numberOfSetBits(uint32_t i) { + return __builtin_popcount(i); +} diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h index 185be0a7..d133638f 100644 --- a/Grbl_Esp32/src/NutsBolts.h +++ b/Grbl_Esp32/src/NutsBolts.h @@ -53,11 +53,6 @@ static inline int toMotor2(int axis) { return axis + MAX_AXES; } -// CoreXY motor assignments. DO NOT ALTER. -// NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations. -#define A_MOTOR X_AXIS // Must be X_AXIS -#define B_MOTOR Y_AXIS // Must be Y_AXIS - // Conversions const double MM_PER_INCH = (25.40); const double INCH_PER_MM = (0.0393701); @@ -111,6 +106,8 @@ float constrain_float(float in, float min, float max); bool char_is_numeric(char value); char* trim(char* value); +int numberOfSetBits(uint32_t i); + template void swap(T& a, T& b) { T c(a); diff --git a/Grbl_Esp32/src/Planner.cpp b/Grbl_Esp32/src/Planner.cpp index 47920960..9c06c77b 100644 --- a/Grbl_Esp32/src/Planner.cpp +++ b/Grbl_Esp32/src/Planner.cpp @@ -38,7 +38,7 @@ typedef struct { // from g-code position for movements requiring multiple line motions, // i.e. arcs, canned cycles, and backlash compensation. float previous_unit_vec[MAX_N_AXIS]; // Unit vector of previous path line segment - float previous_nominal_speed; // Nominal speed of previous path line segment + float previous_nominal_speed; // Nominal speed of previous path line segment } planner_t; static planner_t pl; @@ -313,49 +313,20 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) { uint8_t idx; // Copy position data based on type of motion being planned. if (block->motion.systemMotion) { -#ifdef COREXY - position_steps[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position); - position_steps[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position); - position_steps[Z_AXIS] = sys_position[Z_AXIS]; -#else memcpy(position_steps, sys_position, sizeof(sys_position)); -#endif } else { memcpy(position_steps, pl.position, sizeof(pl.position)); } -#ifdef COREXY - target_steps[A_MOTOR] = lround(target[A_MOTOR] * axis_settings[A_MOTOR]->steps_per_mm->get()); - target_steps[B_MOTOR] = lround(target[B_MOTOR] * axis_settings[B_MOTOR]->steps_per_mm->get()); - block->steps[A_MOTOR] = labs((target_steps[X_AXIS] - position_steps[X_AXIS]) + (target_steps[Y_AXIS] - position_steps[Y_AXIS])); - block->steps[B_MOTOR] = labs((target_steps[X_AXIS] - position_steps[X_AXIS]) - (target_steps[Y_AXIS] - position_steps[Y_AXIS])); -#endif auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { // Calculate target position in absolute steps, number of steps for each axis, and determine max step events. // Also, compute individual axes distance for move and prep unit vector calculations. // NOTE: Computes true distance from converted step values. -#ifdef COREXY - if (!(idx == A_MOTOR) && !(idx == B_MOTOR)) { - target_steps[idx] = lround(target[idx] * axis_settings[idx]->steps_per_mm->get()); - block->steps[idx] = labs(target_steps[idx] - position_steps[idx]); - } - block->step_event_count = MAX(block->step_event_count, block->steps[idx]); - if (idx == A_MOTOR) { - delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] + target_steps[Y_AXIS] - position_steps[Y_AXIS]) / - axis_settings[idx]->steps_per_mm->get(); - } else if (idx == B_MOTOR) { - delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] - target_steps[Y_AXIS] + position_steps[Y_AXIS]) / - axis_settings[idx]->steps_per_mm->get(); - } else { - delta_mm = (target_steps[idx] - position_steps[idx]) / axis_settings[idx]->steps_per_mm->get(); - } -#else target_steps[idx] = lround(target[idx] * axis_settings[idx]->steps_per_mm->get()); block->steps[idx] = labs(target_steps[idx] - position_steps[idx]); block->step_event_count = MAX(block->step_event_count, block->steps[idx]); delta_mm = (target_steps[idx] - position_steps[idx]) / axis_settings[idx]->steps_per_mm->get(); -#endif - unit_vec[idx] = delta_mm; // Store unit vector numerator + unit_vec[idx] = delta_mm; // Store unit vector numerator // Set direction bits. Bit enabled always means direction is negative. if (delta_mm < 0.0) { block->direction_bits |= bit(idx); @@ -456,19 +427,9 @@ void plan_sync_position() { // TODO: For motor configurations not in the same coordinate frame as the machine position, // this function needs to be updated to accomodate the difference. uint8_t idx; - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { -#ifdef COREXY - if (idx == X_AXIS) { - pl.position[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position); - } else if (idx == Y_AXIS) { - pl.position[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position); - } else { - pl.position[idx] = sys_position[idx]; - } -#else - pl.position[idx] = sys_position[idx]; -#endif + pl.position[idx] = sys_position[idx]; } } diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index f1a42673..1a64d945 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -156,9 +156,13 @@ Error list_settings(const char* value, WebUI::AuthenticationLevel auth_level, We } Error list_changed_settings(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { for (Setting* s = Setting::List; s; s = s->next()) { - const char* value = s->getStringValue(); - if (!auth_failed(s, value, auth_level) && strcmp(value, s->getDefaultString())) { - show_setting(s->getName(), value, NULL, out); + const char* value = s->getStringValue(); + const char* defval = s->getDefaultString(); + if (!auth_failed(s, value, auth_level) && strcmp(value, defval)) { + String message = "(Default="; + message += defval; + message += ")"; + show_setting(s->getName(), value, message.c_str(), out); } } grbl_sendf(out->client(), "(Passwords not shown)\r\n"); @@ -285,15 +289,15 @@ Error report_startup_lines(const char* value, WebUI::AuthenticationLevel auth_le std::map restoreCommands = { #ifdef ENABLE_RESTORE_DEFAULT_SETTINGS - { "$", SettingsRestore::Defaults }, { "settings", SettingsRestore::Defaults }, + { "$", SettingsRestore::Defaults }, { "settings", SettingsRestore::Defaults }, #endif #ifdef ENABLE_RESTORE_CLEAR_PARAMETERS - { "#", SettingsRestore::Parameters }, { "gcode", SettingsRestore::Parameters }, + { "#", SettingsRestore::Parameters }, { "gcode", SettingsRestore::Parameters }, #endif #ifdef ENABLE_RESTORE_WIPE_ALL - { "*", SettingsRestore::All }, { "all", SettingsRestore::All }, + { "*", SettingsRestore::All }, { "all", SettingsRestore::All }, #endif - { "@", SettingsRestore::Wifi }, { "wifi", SettingsRestore::Wifi }, + { "@", SettingsRestore::Wifi }, { "wifi", SettingsRestore::Wifi }, }; Error restore_settings(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { if (!value) { @@ -325,12 +329,42 @@ Error doJog(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESP return gc_execute_line(jogLine, out->client()); } -const char* errorString(Error errorNumber) { - auto it = ErrorCodes.find(errorNumber); - return it == ErrorCodes.end() ? NULL : it->second; +const char* alarmString(ExecAlarm alarmNumber) { + auto it = AlarmNames.find(alarmNumber); + return it == AlarmNames.end() ? NULL : it->second; } -Error listErrorCodes(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { +Error listAlarms(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { + if (value) { + char* endptr = NULL; + uint8_t alarmNumber = strtol(value, &endptr, 10); + if (*endptr) { + grbl_sendf(out->client(), "Malformed alarm number: %s\r\n", value); + return Error::InvalidValue; + } + const char* alarmName = alarmString(static_cast(alarmNumber)); + if (alarmName) { + grbl_sendf(out->client(), "%d: %s\r\n", alarmNumber, alarmName); + return Error::Ok; + } else { + grbl_sendf(out->client(), "Unknown alarm number: %d\r\n", alarmNumber); + return Error::InvalidValue; + } + } + + for (auto it = AlarmNames.begin(); it != AlarmNames.end(); it++) { + grbl_sendf(out->client(), "%d: %s\r\n", it->first, it->second); + } + return Error::Ok; +} + + +const char* errorString(Error errorNumber) { + auto it = ErrorNames.find(errorNumber); + return it == ErrorNames.end() ? NULL : it->second; +} + +Error listErrors(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { if (value) { char* endptr = NULL; uint8_t errorNumber = strtol(value, &endptr, 10); @@ -348,7 +382,7 @@ Error listErrorCodes(const char* value, WebUI::AuthenticationLevel auth_level, W } } - for (auto it = ErrorCodes.begin(); it != ErrorCodes.end(); it++) { + for (auto it = ErrorNames.begin(); it != ErrorNames.end(); it++) { grbl_sendf(out->client(), "%d: %s\r\n", it->first, it->second); } return Error::Ok; @@ -381,9 +415,10 @@ void make_grbl_commands() { new GrblCommand("+", "ExtendedSettings/List", report_extended_settings, notCycleOrHold); new GrblCommand("L", "GrblNames/List", list_grbl_names, notCycleOrHold); new GrblCommand("S", "Settings/List", list_settings, notCycleOrHold); - new GrblCommand("SC","Settings/ListChanged", list_changed_settings, notCycleOrHold); + new GrblCommand("SC", "Settings/ListChanged", list_changed_settings, notCycleOrHold); new GrblCommand("CMD", "Commands/List", list_commands, notCycleOrHold); - new GrblCommand("E", "ErrorCodes/List", listErrorCodes, anyState); + new GrblCommand("A", "Alarms/List", listAlarms, anyState); + new GrblCommand("E", "Errors/List", listErrors, anyState); new GrblCommand("G", "GCode/Modes", report_gcode, anyState); new GrblCommand("C", "GCode/Check", toggle_check_mode, anyState); new GrblCommand("X", "Alarm/Disable", disable_alarm_lock, anyState); diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 27ba9ea8..20cff520 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -247,7 +247,14 @@ void report_status_message(Error status_code, uint8_t client) { return; } #endif - grbl_sendf(client, "error:%d\r\n", static_cast(status_code)); + // With verbose errors, the message text is displayed instead of the number. + // Grbl 0.9 used to display the text, while Grbl 1.1 switched to the number. + // Many senders support both formats. + if (verbose_errors->get()) { + grbl_sendf(client, "error: %s\r\n", errorString(status_code)); + } else { + grbl_sendf(client, "error:%d\r\n", static_cast(status_code)); + } } } @@ -519,9 +526,6 @@ void report_build_info(const char* line, uint8_t client) { #ifdef COOLANT_MIST_PIN grbl_send(client, "M"); // TODO Need to deal with M8...it could be disabled #endif -#ifdef COREXY - grbl_send(client, "C"); -#endif #ifdef PARKING_ENABLE grbl_send(client, "P"); #endif @@ -941,7 +945,7 @@ char* reportAxisNameMsg(uint8_t axis) { void reportTaskStackSize(UBaseType_t& saved) { #ifdef DEBUG_REPORT_STACK_FREE - UBaseType_t newHighWater = uxTaskGetStackHighWaterMark(NULL); + UBaseType_t newHighWater = uxTaskGetStackHighWaterMark(NULL); if (newHighWater != saved) { saved = newHighWater; grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Min Stack Space: %d", pcTaskGetTaskName(NULL), saved); diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index 5e356d99..5450b444 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -233,7 +233,7 @@ const char* AxisMaskSetting::getCompatibleValue() { } static char* maskToString(uint32_t mask, char* strval) { - char* s = strval; + char* s = strval; for (int i = 0; i < MAX_N_AXIS; i++) { if (mask & bit(i)) { *s++ = "XYZABC"[i]; @@ -566,7 +566,7 @@ void FlagSetting::setDefault() { } Error FlagSetting::setStringValue(char* s) { - s = trim(s); + s = trim(s); Error err = check(s); if (err != Error::Ok) { return err; diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index c513b8b4..63dce4f5 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -10,12 +10,12 @@ void settings_init(); // Define settings restore bitflags. enum SettingsRestore { - Defaults = bit(0), - Parameters = bit(1), + Defaults = bit(0), + Parameters = bit(1), StartupLines = bit(2), // BuildInfo = bit(3), // Obsolete Wifi = bit(4), - All = 0xff, + All = 0xff, }; // Restore subsets of settings to default values @@ -101,9 +101,9 @@ protected: public: static nvs_handle _handle; - static void init(); - static Setting* List; - Setting* next() { return link; } + static void init(); + static Setting* List; + Setting* next() { return link; } Error check(char* s); @@ -227,22 +227,25 @@ public: class Coordinates { private: - float _currentValue[MAX_N_AXIS]; + float _currentValue[MAX_N_AXIS]; const char* _name; + public: Coordinates(const char* name) : _name(name) {} const char* getName() { return _name; } - bool load(); - void setDefault() { - float zeros[MAX_N_AXIS] = { 0.0, }; + bool load(); + void setDefault() { + float zeros[MAX_N_AXIS] = { + 0.0, + }; set(zeros); }; // Copy the value to an array void get(float* value) { memcpy(value, _currentValue, sizeof(_currentValue)); } // Return a pointer to the array const float* get() { return _currentValue; } - void set(float *value); + void set(float* value); }; extern Coordinates* coords[CoordIndex::End]; diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index ef5f0f5a..5e2c2e2c 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -2,6 +2,8 @@ bool motorSettingChanged = false; +FlagSetting* verbose_errors; + FakeSetting* number_axis; StringSetting* startup_line_0; @@ -35,19 +37,19 @@ IntSetting* status_mask; FloatSetting* junction_deviation; FloatSetting* arc_tolerance; -FloatSetting* homing_feed_rate; -FloatSetting* homing_seek_rate; -FloatSetting* homing_debounce; -FloatSetting* homing_pulloff; +FloatSetting* homing_feed_rate; +FloatSetting* homing_seek_rate; +FloatSetting* homing_debounce; +FloatSetting* homing_pulloff; AxisMaskSetting* homing_cycle[MAX_N_AXIS]; -FloatSetting* spindle_pwm_freq; -FloatSetting* rpm_max; -FloatSetting* rpm_min; -FloatSetting* spindle_delay_spinup; -FloatSetting* spindle_delay_spindown; -FlagSetting* spindle_enbl_off_with_zero_speed; -FlagSetting* spindle_enable_invert; -FlagSetting* spindle_output_invert; +FloatSetting* spindle_pwm_freq; +FloatSetting* rpm_max; +FloatSetting* rpm_min; +FloatSetting* spindle_delay_spinup; +FloatSetting* spindle_delay_spindown; +FlagSetting* spindle_enbl_off_with_zero_speed; +FlagSetting* spindle_enable_invert; +FlagSetting* spindle_output_invert; FloatSetting* spindle_pwm_off_value; FloatSetting* spindle_pwm_min_value; @@ -237,6 +239,8 @@ void make_settings() { make_coordinate(CoordIndex::G28, "G28"); make_coordinate(CoordIndex::G30, "G30"); + verbose_errors = new FlagSetting(EXTENDED, WG, NULL, "Errors/Verbose", DEFAULT_VERBOSE_ERRORS); + // number_axis = new IntSetting(EXTENDED, WG, NULL, "NumberAxis", N_AXIS, 0, 6, NULL, true); number_axis = new FakeSetting(N_AXIS); diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index 0fd68b33..b01038bf 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -2,6 +2,8 @@ extern bool motorSettingChanged; +extern FlagSetting* verbose_errors; + extern FakeSetting* number_axis; extern AxisSettings* x_axis_settings; diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index d8d18d00..498915f1 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -166,17 +166,7 @@ void system_flag_wco_change() { float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx) { float pos; float steps_per_mm = axis_settings[idx]->steps_per_mm->get(); -#ifdef COREXY - if (idx == X_AXIS) { - pos = (float)system_convert_corexy_to_x_axis_steps(steps) / steps_per_mm; - } else if (idx == Y_AXIS) { - pos = (float)system_convert_corexy_to_y_axis_steps(steps) / steps_per_mm; - } else { - pos = steps[idx] / steps_per_mm; - } -#else - pos = steps[idx] / steps_per_mm; -#endif + pos = steps[idx] / steps_per_mm; return pos; } @@ -275,15 +265,6 @@ void system_exec_control_pin(ControlPins pins) { } } -// CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps. -int32_t system_convert_corexy_to_x_axis_steps(int32_t* steps) { - return (steps[A_MOTOR] + steps[B_MOTOR]) / 2; -} - -int32_t system_convert_corexy_to_y_axis_steps(int32_t* steps) { - return (steps[A_MOTOR] - steps[B_MOTOR]) / 2; -} - // io_num is the virtual pin# and has nothing to do with the actual esp32 GPIO_NUM_xx // It uses a mask so all can be turned of in ms_reset bool sys_io_control(uint8_t io_num_mask, bool turnOn, bool synchronized) { diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index e1ea051a..6803f7e2 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -20,6 +20,9 @@ along with Grbl. If not, see . */ +// Execution states and alarm +#include "Exec.h" + // System states. The state variable primarily tracks the individual functions // of Grbl to manage each without overlapping. It is also used as a messaging flag for // critical events. @@ -107,56 +110,6 @@ typedef struct { } system_t; extern system_t sys; -// System executor bit map. Used internally by realtime protocol as realtime command flags, -// which notifies the main program to execute the specified realtime command asynchronously. -// NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default -// flags are always false, so the realtime protocol only needs to check for a non-zero value to -// know when there is a realtime command to execute. -struct ExecStateBits { - uint8_t statusReport : 1; - uint8_t cycleStart : 1; - uint8_t cycleStop : 1; // Unused, per cycle_stop variable - uint8_t feedHold : 1; - uint8_t reset : 1; - uint8_t safetyDoor : 1; - uint8_t motionCancel : 1; - uint8_t sleep : 1; -}; - -union ExecState { - uint8_t value; - ExecStateBits bit; -}; - -// Alarm executor codes. Valid values (1-255). Zero is reserved. -enum class ExecAlarm : uint8_t { - None = 0, - HardLimit = 1, - SoftLimit = 2, - AbortCycle = 3, - ProbeFailInitial = 4, - ProbeFailContact = 5, - HomingFailReset = 6, - HomingFailDoor = 7, - HomingFailPulloff = 8, - HomingFailApproach = 9, - SpindleControl = 10, -}; - -// Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides. -// Spindle/coolant and feed/rapids are separated into two controlling flag variables. - -struct ExecAccessoryBits { - uint8_t spindleOvrStop : 1; - uint8_t coolantFloodOvrToggle : 1; - uint8_t coolantMistOvrToggle : 1; -}; - -union ExecAccessory { - uint8_t value; - ExecAccessoryBits bit; -}; - // Control pin states struct ControlPinBits { uint8_t safetyDoor : 1; @@ -213,9 +166,6 @@ float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx); // Updates a machine 'position' array based on the 'step' array sent. void system_convert_array_steps_to_mpos(float* position, int32_t* steps); -int32_t system_convert_corexy_to_x_axis_steps(int32_t* steps); -int32_t system_convert_corexy_to_y_axis_steps(int32_t* steps); - // A task that runs after a control switch interrupt for debouncing. void controlCheckTask(void* pvParameters); void system_exec_control_pin(ControlPins pins); diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index 09539a77..b9a34b8c 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -306,9 +306,9 @@ namespace WebUI { return Error::SdFailedOpenFile; } //until no line in file - Error err; - Error accumErr = Error::Ok; - uint8_t client = (espresponse) ? espresponse->client() : CLIENT_ALL; + Error err; + Error accumErr = Error::Ok; + uint8_t client = (espresponse) ? espresponse->client() : CLIENT_ALL; while (currentfile.available()) { String currentline = currentfile.readStringUntil('\n'); if (currentline.length() > 0) { @@ -398,7 +398,7 @@ namespace WebUI { user_password->setDefault(); return Error::Ok; } - if (user_password->setStringValue(parameter)) { + if (user_password->setStringValue(parameter) != Error::Ok) { webPrintln("Invalid Password"); return Error::InvalidValue; } @@ -730,7 +730,7 @@ namespace WebUI { webPrintln(""); return Error::Ok; } - SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL; + SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL; SD_auth_level = auth_level; // execute the first line now; Protocol.cpp handles later ones when SD_ready_next report_status_message(execute_line(fileLine, SD_client, SD_auth_level), SD_client); From 4b7ac102a99bef49b1c9a15142d44f044e94c8fa Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 23 Nov 2020 08:49:48 -0600 Subject: [PATCH 2/2] Delete esp32_printer_controller.cpp --- .../Custom/esp32_printer_controller.cpp | 183 ------------------ 1 file changed, 183 deletions(-) delete mode 100644 Grbl_Esp32/Custom/esp32_printer_controller.cpp diff --git a/Grbl_Esp32/Custom/esp32_printer_controller.cpp b/Grbl_Esp32/Custom/esp32_printer_controller.cpp deleted file mode 100644 index 6c2a8cd1..00000000 --- a/Grbl_Esp32/Custom/esp32_printer_controller.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* - esp32_printer_controller.cpp (copy and use your machine name) - Part of Grbl_ESP32 - - copyright (c) 2020 - Bart Dring. This file was intended for use on the ESP32 - - ...add your date and name here. - - CPU. Do not use this with Grbl for atMega328P - - 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_ESP32 is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . - - ======================================================================= - -This is a template for user-defined C++ code functions. Grbl can be -configured to call some optional functions, enabled by #define statements -in the machine definition .h file. Implement the functions thus enabled -herein. The possible functions are listed and described below. - -To use this file, copy it to a name of your own choosing, and also copy -Machines/template.h to a similar name. - -Example: -Machines/my_machine.h -Custom/my_machine.cpp - -Edit machine.h to include your Machines/my_machine.h file - -Edit Machines/my_machine.h according to the instructions therein. - -Fill in the function definitions below for the functions that you have -enabled with USE_ defines in Machines/my_machine.h - -=============================================================================== - -*/ - -#ifdef USE_MACHINE_INIT -/* -machine_init() is called when Grbl_ESP32 first starts. You can use it to do any -special things your machine needs at startup. -*/ -# define STEPPERS_DISABLE_PIN_X 138 -# define STEPPERS_DISABLE_PIN_Y 134 -# define STEPPERS_DISABLE_PIN_Z 131 -# define STEPPERS_DISABLE_PIN_A 139 - -# define FAN1_PIN 13 -# define FAN2_PIN 142 -# define FAN3_PIN 143 - -# define BED_PIN 4 -# define NOZZLE_PIN 2 - -void machine_init() { - // Enable steppers - digitalWrite(STEPPERS_DISABLE_PIN_X, LOW); // enable - digitalWrite(STEPPERS_DISABLE_PIN_Y, LOW); // enable - digitalWrite(STEPPERS_DISABLE_PIN_Z, LOW); // enable - digitalWrite(STEPPERS_DISABLE_PIN_A, LOW); // enable - - // digitalWrite(FAN1_PIN, LOW); // comment out for JTAG debugging - - digitalWrite(FAN2_PIN, LOW); // disable - digitalWrite(FAN3_PIN, LOW); // disable - - digitalWrite(BED_PIN, LOW); // disable - digitalWrite(NOZZLE_PIN, LOW); // disable -} -#endif - -#ifdef USE_CUSTOM_HOMING -/* - (user_defined_homing) is called at the begining of the normal Grbl_ESP32 homing - sequence. If user_defined_homing() returns false, the rest of normal Grbl_ESP32 - homing is skipped if it returns false, other normal homing continues. For - example, if you need to manually prep the machine for homing, you could implement - user_defined_homing() to wait for some button to be pressed, then return true. -*/ -bool user_defined_homing(uint8_t cycle_mask) { - // True = done with homing, false = continue with normal Grbl_ESP32 homing - return true; -} -#endif - -#ifdef USE_KINEMATICS -/* - Inverse Kinematics converts X,Y,Z cartesian coordinate to the steps - on your "joint" motors. It requires the following three functions: -*/ - -/* - inverse_kinematics() looks at incoming move commands and modifies - them before Grbl_ESP32 puts them in the motion planner. - - Grbl_ESP32 processes arcs by converting them into tiny little line segments. - Kinematics in Grbl_ESP32 works the same way. Search for this function across - Grbl_ESP32 for examples. You are basically converting cartesian X,Y,Z... targets to - - target = an N_AXIS array of target positions (where the move is supposed to go) - pl_data = planner data (see the definition of this type to see what it is) - position = an N_AXIS array of where the machine is starting from for this move -*/ -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) { - // this simply moves to the target. Replace with your kinematics. - mc_line(target, pl_data); -} - -/* - kinematics_pre_homing() is called before normal homing - You can use it to do special homing or just to set stuff up - - cycle_mask is a bit mask of the axes being homed this time. -*/ -bool kinematics_pre_homing(uint8_t cycle_mask)) -{ - return false; // finish normal homing cycle -} - -/* - kinematics_post_homing() is called at the end of normal homing -*/ -void kinematics_post_homing() {} -#endif - -#ifdef USE_FWD_KINEMATICS -/* - The status command uses forward_kinematics() to convert - your motor positions to cartesian X,Y,Z... coordinates. - - Convert the N_AXIS array of motor positions to cartesian in your code. -*/ -void forward_kinematics(float* position) { - // position[X_AXIS] = - // position[Y_AXIS] = -} -#endif - -#ifdef USE_TOOL_CHANGE -/* - user_tool_change() is called when tool change gcode is received, - to perform appropriate actions for your machine. -*/ -void user_tool_change(uint8_t new_tool) {} -#endif - -#if defined(MACRO_BUTTON_0_PIN) || defined(MACRO_BUTTON_1_PIN) || defined(MACRO_BUTTON_2_PIN) -/* - options. user_defined_macro() is called with the button number to - perform whatever actions you choose. -*/ -void user_defined_macro(uint8_t index) {} -#endif - -#ifdef USE_M30 -/* - user_m30() is called when an M30 gcode signals the end of a gcode file. -*/ -void user_m30() {} -#endif - -#ifdef USE_MACHINE_TRINAMIC_INIT -/* - machine_triaminic_setup() replaces the normal setup of trinamic - drivers with your own code. For example, you could setup StallGuard -*/ -void machine_trinamic_setup() {} -#endif - -// If you add any additional functions specific to your machine that -// require calls from common code, guard their calls in the common code with -// #ifdef USE_WHATEVER and add function prototypes (also guarded) to grbl.h