From f3ca8b24467f7f39342ad4ff49f982b7f415cb29 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 25 Apr 2021 08:26:47 -0500 Subject: [PATCH] Devt (#882) * Oled2 (#834) * WIP * WIP * Update platformio.ini * WIP * Cleanup * Update platformio.ini * Turn off soft limits with max travel (#836) https://github.com/bdring/Grbl_Esp32/issues/831 * Yalang YL620 VFD (#838) * New SpindleType YL620 Files for new SpindleType Yalang 620. So far the contents are a duplicate of H2ASpindle.h and H2ASpindle.cpp * Added register documentation and implemented read and write data packets * Some fixes, mostly regarding RX packet length * OLED and Other Updates (#844) * publish * Updates - CoreXY and OLED - Moved position calculation out of report_realtime_status(...) so other functions can access it. - Added a function to check if a limit switch is defined - CoreXY fixed bug in forward kinematics when midtbot is used. - Modified OLED display. * Cleanup for PR * Delete midtbot_x2.h * Incorporated PR 846 - Some OLED cleanup - verified correct forward kinematics on MidTbot * Pio down rev (#850) * Update platformio.ini * Update Grbl.h * Use local UART driver not HardwareSerial (#857) * Use local UART driver not HardwareSerial The HardwareSerial driver is broken in Arduino framework versions 1.0.5 and 1.0.6 . https://github.com/espressif/arduino-esp32/issues/5005 Instead of waiting for a fix, I wrote a very simple UART driver that does exactly what we need with no unnecessary bells and whistles to cause problems. * Added missing files, changed method signatures The methods implemented by the UART class now have the same signatures as the HardwareSerial class, so it will be easy to switch back if we need to. * Incorporated suggestions from Stefan * Fixed TX_IDLE_NUM bug reported by mstrens * Quick test for Bf: problem This is not the final solution. * Fixed stupid typo in last commit * Another test - check for client_buffer space * Use the esp-idf uart driver You can revert to the direct driver for testing by defining DIRECT_UART * Uart class now supports VFD and TMC * data bits, stop bits, parity as enum classes The constants for data bits, stop bits, and parity were changed to enum classes so the compiler can check for argument order mismatches. * Set half duplex after uart init * Init TMC UART only once * rx/tx pin order mixup, missing _uart_started * Test: use Arduino Serial This reverts to the Arduino serial driver for UI communication, leaving the VFS comms on the Uart class on top of the esp_idf UART driver. You can switch back and forth with the define REVERT_TO_SERIAL line in Serial.cpp * REVERT_TO_ARDUINO_SERIAL off by default * Added debug messages * Update Grbl.h * Update platformio.ini Co-authored-by: bdring * Fixed spindle sync for all VFD spindles (#868) * Implemented H2A spindle sync fix. Untested. * Changed the spindle sync fix to be in the VFD code. * Update Grbl.h Co-authored-by: Stefan de Bruijn Co-authored-by: bdring * New jog fix (#872) * Applied 741 to new Devt * Make kinematics routines weak to eliminate ifdefs * Fixed warning * Update build date Co-authored-by: bdring * Big kinematics cleanup (#875) * Applied 741 to new Devt * Make kinematics routines weak to eliminate ifdefs * Fixed warning * Big kinematics cleanup * Cleanup * no isCancelled arg for jog_execute; return it instead * WIP * Made OLED compliant with new kinematics * Added system_get_mpos * system_get_mpos() returns float* * WIP * Cleanup after testing - Had MPos and WPos text on OLED backwards. - Added my cartesian test def - Will remove test defs before merging to devt. * Cleanup of remaining user optional code. * Fixed delta kinematics loop ending early. * Account for jog cancel in saved motor positions * Update Grbl.h Co-authored-by: bdring Co-authored-by: Mitch Bradley Co-authored-by: marcosprojects Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn --- Grbl_Esp32/Custom/CoreXY.cpp | 106 +++++------ Grbl_Esp32/Custom/atari_1020.cpp | 5 +- Grbl_Esp32/Custom/custom_code_template.cpp | 75 +++----- Grbl_Esp32/Custom/mpcnc_laser_module.cpp | 2 - Grbl_Esp32/Custom/oled_basic.cpp | 9 +- Grbl_Esp32/Custom/parallel_delta.cpp | 169 +++++++----------- Grbl_Esp32/Custom/polar_coaster.cpp | 31 ++-- Grbl_Esp32/src/Error.cpp | 1 + Grbl_Esp32/src/Error.h | 1 + Grbl_Esp32/src/GCode.cpp | 30 ++-- Grbl_Esp32/src/Grbl.cpp | 8 + Grbl_Esp32/src/Grbl.h | 28 ++- Grbl_Esp32/src/Jog.cpp | 14 +- Grbl_Esp32/src/Jog.h | 3 +- Grbl_Esp32/src/Limits.cpp | 5 +- .../src/Machines/6_pack_TMC2130_XYZ_Test.h | 110 ++++++++++++ Grbl_Esp32/src/Machines/atari_1020.h | 4 - Grbl_Esp32/src/Machines/fysetc_ant.h | 85 +++++++++ .../src/Machines/{fystec_e4.h => fysetc_e4.h} | 6 +- Grbl_Esp32/src/Machines/midtbot.h | 8 +- Grbl_Esp32/src/Machines/midtbot_x2.h | 119 ++++++++++++ .../src/Machines/mpcnc_laser_module_v1p2.h | 1 - Grbl_Esp32/src/Machines/polar_coaster.h | 7 +- Grbl_Esp32/src/Machines/tapster_3.h | 6 - .../src/Machines/tapster_pro_6P_trinamic.h | 6 - Grbl_Esp32/src/MotionControl.cpp | 59 +++--- Grbl_Esp32/src/MotionControl.h | 4 +- Grbl_Esp32/src/Motors/Dynamixel2.cpp | 17 +- Grbl_Esp32/src/Motors/Motor.cpp | 4 +- Grbl_Esp32/src/Motors/Motors.cpp | 1 - Grbl_Esp32/src/Planner.h | 1 + Grbl_Esp32/src/Protocol.cpp | 13 +- Grbl_Esp32/src/Report.cpp | 57 ++---- Grbl_Esp32/src/Report.h | 6 +- Grbl_Esp32/src/System.cpp | 22 ++- Grbl_Esp32/src/System.h | 4 +- 36 files changed, 605 insertions(+), 422 deletions(-) create mode 100644 Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h create mode 100644 Grbl_Esp32/src/Machines/fysetc_ant.h rename Grbl_Esp32/src/Machines/{fystec_e4.h => fysetc_e4.h} (95%) create mode 100644 Grbl_Esp32/src/Machines/midtbot_x2.h diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp index 0acad5b4..39488722 100644 --- a/Grbl_Esp32/Custom/CoreXY.cpp +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -43,7 +43,6 @@ const float geometry_factor = 1.0; 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 @@ -59,6 +58,19 @@ void machine_init() { #endif } +// Converts Cartesian to motors with no motion control +static void cartesian_to_motors(float* position) { + float motors[MAX_N_AXIS]; + + motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; + motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; + + position[X_AXIS] = motors[X_AXIS]; + position[Y_AXIS] = motors[Y_AXIS]; + + // Z and higher just pass through unchanged +} + // Cycle mask is 0 unless the user sends a single axis command like $HZ // This will always return true to prevent the normal Grbl homing cycle bool user_defined_homing(uint8_t cycle_mask) { @@ -131,11 +143,11 @@ bool user_defined_homing(uint8_t cycle_mask) { } for (int axis = Z_AXIS; axis < n_axis; axis++) { - target[axis] = system_convert_axis_steps_to_mpos(sys_position, axis); + target[axis] = sys_position[axis] / axis_settings[axis]->steps_per_mm->get(); } // convert back to motor steps - inverse_kinematics(target); + cartesian_to_motors(target); pl_data->feed_rate = homing_rate; // feed or seek rates plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion. @@ -222,11 +234,11 @@ bool user_defined_homing(uint8_t cycle_mask) { last_cartesian[Y_AXIS] = target[Y_AXIS]; for (int axis = Z_AXIS; axis < n_axis; axis++) { - last_cartesian[axis] = system_convert_axis_steps_to_mpos(sys_position, axis); + last_cartesian[axis] = sys_position[axis] / axis_settings[axis]->steps_per_mm->get(); } // convert to motors - inverse_kinematics(target); + cartesian_to_motors(target); // convert to steps for (axis = X_AXIS; axis <= Y_AXIS; axis++) { sys_position[axis] = target[axis] * axis_settings[axis]->steps_per_mm->get(); @@ -242,29 +254,21 @@ bool user_defined_homing(uint8_t cycle_mask) { 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[MAX_N_AXIS]; +static void transform_cartesian_to_motors(float* motors, float* cartesian) { + motors[X_AXIS] = geometry_factor * cartesian[X_AXIS] + cartesian[Y_AXIS]; + motors[Y_AXIS] = geometry_factor * cartesian[X_AXIS] - cartesian[Y_AXIS]; - motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; - motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; - - position[X_AXIS] = motors[X_AXIS]; - position[Y_AXIS] = motors[Y_AXIS]; - - // Z and higher just pass through unchanged + auto n_axis = number_axis->get(); + for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) { + motors[axis] = cartesian[axis]; + } } // Inverse Kinematics calculates motor positions from real world cartesian positions -// position is the current position +// position is the old machine position, target the new machine 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 -{ +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { 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]; @@ -272,56 +276,30 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio 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]; - auto n_axis = number_axis->get(); - for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) { - motors[axis] = target[axis]; - } - float motor_distance = three_axis_dist(motors, last_motors); + float motors[n_axis]; + transform_cartesian_to_motors(motors, target); if (!pl_data->motion.rapidMotion) { - pl_data->feed_rate *= (motor_distance / dist); + float last_motors[n_axis]; + transform_cartesian_to_motors(last_motors, position); + pl_data->feed_rate *= (three_axis_dist(motors, last_motors) / dist); } - memcpy(last_motors, motors, sizeof(motors)); - - mc_line(motors, pl_data); + return mc_line(motors, pl_data); } // motors -> cartesian -void forward_kinematics(float* position) { - float calc_fwd[MAX_N_AXIS]; - float wco[MAX_N_AXIS]; - float print_position[N_AXIS]; - int32_t current_position[N_AXIS]; // Copy current state of the system position variable - - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - - // determine the Work Coordinate Offsets for each axis - auto n_axis = number_axis->get(); - for (int axis = 0; axis < n_axis; axis++) { - // Apply work coordinate offsets and tool length offset to current position. - wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis]; - if (axis == TOOL_LENGTH_OFFSET_AXIS) { - wco[axis] += gc_state.tool_length_offset; - } - } - +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { // apply the forward kinemetics to the machine coordinates // https://corexy.com/theory.html //calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]); - calc_fwd[X_AXIS] = ((0.5 * (print_position[X_AXIS] + print_position[Y_AXIS]) / geometry_factor) - wco[X_AXIS]); - calc_fwd[Y_AXIS] = ((0.5 * (print_position[X_AXIS] - print_position[Y_AXIS])) - wco[Y_AXIS]); + cartesian[X_AXIS] = 0.5 * (motors[X_AXIS] + motors[Y_AXIS]) / geometry_factor; + cartesian[Y_AXIS] = 0.5 * (motors[X_AXIS] - motors[Y_AXIS]); - for (int axis = 0; axis < n_axis; axis++) { - if (axis > Y_AXIS) { // for axes above Y there is no kinematics - calc_fwd[axis] = print_position[axis] - wco[axis]; - } - position[axis] = calc_fwd[axis]; // this is the value returned to reporting + for (int axis = Z_AXIS; axis < n_axis; axis++) { + cartesian[axis] = motors[axis]; } } @@ -331,14 +309,12 @@ bool kinematics_pre_homing(uint8_t cycle_mask) { void kinematics_post_homing() { auto n_axis = number_axis->get(); - for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) { - gc_state.position[axis] = last_cartesian[axis]; - } + memcpy(gc_state.position, last_cartesian, n_axis * sizeof(last_cartesian[0])); } -// 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; +// this is used used by Limits.cpp to see if the range of the machine is exceeded. +bool limitsCheckTravel(float* target) { + return false; } void user_m30() {} diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp index 27a55021..8f27b4da 100644 --- a/Grbl_Esp32/Custom/atari_1020.cpp +++ b/Grbl_Esp32/Custom/atari_1020.cpp @@ -75,15 +75,12 @@ void machine_init() { // this task tracks the Z position and sets the solenoid void solenoidSyncTask(void* pvParameters) { int32_t current_position[N_AXIS]; // copy of current location - float m_pos[N_AXIS]; // machine position in mm TickType_t xLastWakeTime; const TickType_t xSolenoidFrequency = SOLENOID_TASK_FREQ; // in ticks (typically ms) xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. while (true) { // don't ever return from this or the task dies - memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step - system_convert_array_steps_to_mpos(m_pos, current_position); // convert to millimeters - calc_solenoid(m_pos[Z_AXIS]); // calculate kinematics and move the servos + calc_solenoid(system_get_mpos()[Z_AXIS]); // calculate kinematics and move the servos vTaskDelayUntil(&xLastWakeTime, xSolenoidFrequency); } } diff --git a/Grbl_Esp32/Custom/custom_code_template.cpp b/Grbl_Esp32/Custom/custom_code_template.cpp index bd468f6d..54d5f6ec 100644 --- a/Grbl_Esp32/Custom/custom_code_template.cpp +++ b/Grbl_Esp32/Custom/custom_code_template.cpp @@ -24,37 +24,39 @@ ======================================================================= 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. +configured to call some optional functions. These functions have weak definitions +in the main code. If you create your own version they will be used instead -To use this file, copy it to a name of your own choosing, and also copy -Machines/template.h to a similar name. +Put all of your functions in a .cpp file in the "Custom" folder. +Add this to your machine definition file +#define CUSTOM_CODE_FILENAME "../Custom/YourFile.cpp" -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 +Be careful to return the correct values =============================================================================== +Below are all the current weak function + */ -#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. +This function is used as a one time setup for your machine. */ void machine_init() {} -#endif -#ifdef USE_CUSTOM_HOMING +/* +This is used to initialize a display. +*/ +void display_init() {} + +/* + limitsCheckTravel() is called to check soft limits + It returns true if the motion is outside the limit values +*/ +bool limitsCheckTravel() { + return false; +} + /* 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 @@ -66,17 +68,14 @@ 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. + cartesian_to_motors() converts from cartesian coordinates to motor space. 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 @@ -86,9 +85,9 @@ bool user_defined_homing(uint8_t cycle_mask) { 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) { +bool cartesian_to_motors(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); + return mc_line(target, pl_data); } /* @@ -97,8 +96,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio cycle_mask is a bit mask of the axes being homed this time. */ -bool kinematics_pre_homing(uint8_t cycle_mask)) -{ +bool kinematics_pre_homing(uint8_t cycle_mask) { return false; // finish normal homing cycle } @@ -106,51 +104,34 @@ bool kinematics_pre_homing(uint8_t cycle_mask)) 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 + The status command uses motors_to_cartesian() 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) { +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { // 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 diff --git a/Grbl_Esp32/Custom/mpcnc_laser_module.cpp b/Grbl_Esp32/Custom/mpcnc_laser_module.cpp index e65ce629..52b816c4 100644 --- a/Grbl_Esp32/Custom/mpcnc_laser_module.cpp +++ b/Grbl_Esp32/Custom/mpcnc_laser_module.cpp @@ -21,7 +21,6 @@ along with Grbl. If not, see . */ -#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. @@ -32,4 +31,3 @@ void machine_init() { pinMode(LVL_SHIFT_ENABLE, OUTPUT); digitalWrite(LVL_SHIFT_ENABLE, HIGH); } -#endif diff --git a/Grbl_Esp32/Custom/oled_basic.cpp b/Grbl_Esp32/Custom/oled_basic.cpp index fedc543e..b1e2862b 100644 --- a/Grbl_Esp32/Custom/oled_basic.cpp +++ b/Grbl_Esp32/Custom/oled_basic.cpp @@ -120,7 +120,6 @@ void draw_checkbox(int16_t x, int16_t y, int16_t width, int16_t height, bool che void displayDRO() { uint8_t oled_y_pos; - float print_position[MAX_N_AXIS]; //float wco[MAX_N_AXIS]; display.setTextAlignment(TEXT_ALIGN_LEFT); @@ -135,10 +134,14 @@ void displayDRO() { ControlPins ctrl_pin_state = system_control_get_state(); bool prb_pin_state = probe_get_state(); + display.setTextAlignment(TEXT_ALIGN_RIGHT); + + float* print_position = system_get_mpos(); if (bit_istrue(status_mask->get(), RtStatus::Position)) { - calc_mpos(print_position); + display.drawString(60, 14, "M Pos"); } else { - calc_wpos(print_position); + display.drawString(60, 14, "W Pos"); + mpos_to_wpos(print_position); } for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp index a82de59b..23b28a81 100644 --- a/Grbl_Esp32/Custom/parallel_delta.cpp +++ b/Grbl_Esp32/Custom/parallel_delta.cpp @@ -91,7 +91,6 @@ static float last_cartesian[N_AXIS] = { }; // 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); @@ -108,11 +107,9 @@ void machine_init() { 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 + motors_to_cartesian(cartesian, angles, 3); // 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]); @@ -126,32 +123,20 @@ void machine_init() { // DXL_COUNT_MAX, // DXL_COUNT_PER_RADIAN); } -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]; +// 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 +// } - 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 -{ +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { float dx, dy, dz; // distances in each cartesian axis float motor_angles[3]; - float seg_target[3]; // The target of the current segment + 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 @@ -162,16 +147,17 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // 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); + status = delta_calcInverse(position, last_angle); if (status == KinematicError::OUT_OF_RANGE) { //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; + return false; } // 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 error %3.3f %3.3f %3.3f", target[0], target[1], target[2]); + return false; } position[X_AXIS] += gc_state.coord_offset[X_AXIS]; @@ -198,21 +184,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // calculate the delta motor angles KinematicError status = delta_calcInverse(seg_target, motor_angles); - if (status == KinematicError ::NONE) { - float delta_distance = three_axis_dist(motor_angles, last_angle); - - // save angles for next distance calc - memcpy(last_angle, motor_angles, sizeof(motor_angles)); - - if (pl_data->motion.rapidMotion) { - pl_data->feed_rate = feed_rate; - } else { - pl_data->feed_rate = (feed_rate * delta_distance / segment_dist); - } - - mc_line(motor_angles, pl_data); - - } else { + if (status != KinematicError ::NONE) { if (show_error) { // grbl_msg_sendf(CLIENT_SERIAL, // MsgLevel::Info, @@ -223,35 +195,52 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // motor_angles[2]); show_error = false; } + return false; } + if (pl_data->motion.rapidMotion) { + pl_data->feed_rate = feed_rate; + } else { + float delta_distance = three_axis_dist(motor_angles, last_angle); + pl_data->feed_rate = (feed_rate * delta_distance / segment_dist); + } + + // mc_line() returns false if a jog is cancelled. + // In that case we stop sending segments to the planner. + if (!mc_line(motor_angles, pl_data)) { + return false; + } + + // save angles for next distance calc + // This is after mc_line() so that we do not update + // last_angle if the segment was discarded. + memcpy(last_angle, motor_angles, sizeof(motor_angles)); } + return true; } // 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) { +bool limitsCheckTravel(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) { + switch (delta_calcInverse(target, motor_angles)) { case KinematicError::OUT_OF_RANGE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target out of range"); - break; + return true; case KinematicError::ANGLE_TOO_NEGATIVE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max negative"); - break; + return true; case KinematicError::ANGLE_TOO_POSITIVE: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max positive"); - break; + return true; case KinematicError::NONE: - break; + return false; } - return (status == KinematicError::NONE); + return false; } // inverse kinematics: cartesian -> angles @@ -286,19 +275,21 @@ KinematicError delta_calcInverse(float* cartesian, float* angles) { } // inverse kinematics: angles -> cartesian -int calc_forward_kinematics(float* angles, float* catesian) { +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + read_settings(); + float t = (f - e) * tan30 / 2; - float y1 = -(t + rf * cos(angles[0])); - float z1 = -rf * sin(angles[0]); + float y1 = -(t + rf * cos(motors[0])); + float z1 = -rf * sin(motors[0]); - float y2 = (t + rf * cos(angles[1])) * sin30; + float y2 = (t + rf * cos(motors[1])) * sin30; float x2 = y2 * tan60; - float z2 = -rf * sin(angles[1]); + float z2 = -rf * sin(motors[1]); - float y3 = (t + rf * cos(angles[2])) * sin30; + float y3 = (t + rf * cos(motors[2])) * sin30; float x3 = -y3 * tan60; - float z3 = -rf * sin(angles[2]); + float z3 = -rf * sin(motors[2]); float dnm = (y2 - y1) * x3 - (y3 - y1) * x2; @@ -321,14 +312,16 @@ int calc_forward_kinematics(float* angles, float* catesian) { // discriminant float d = b * b - (float)4.0 * a * c; - if (d < 0) - return -1; // non-existing point + if (d < 0) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "MSG:Fwd Kin Error"); + return; + } - catesian[Z_AXIS] = -(float)0.5 * (b + sqrt(d)) / a; - catesian[X_AXIS] = (a1 * catesian[Z_AXIS] + b1) / dnm; - catesian[Y_AXIS] = (a2 * catesian[Z_AXIS] + b2) / dnm; - return 0; + cartesian[Z_AXIS] = -(float)0.5 * (b + sqrt(d)) / a; + cartesian[X_AXIS] = (a1 * cartesian[Z_AXIS] + b1) / dnm; + cartesian[Y_AXIS] = (a2 * cartesian[Z_AXIS] + b2) / dnm; } + // helper functions, calculates angle theta1 (for YZ-pane) KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta) { float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 @@ -361,43 +354,15 @@ 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]))); } -// called by reporting for WPos status -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); - - if (status == 0) { - // apply offsets and set the returned values - position[X_AXIS] = calc_fwd[X_AXIS] - gc_state.coord_system[X_AXIS] + gc_state.coord_offset[X_AXIS]; - position[Y_AXIS] = calc_fwd[Y_AXIS] - gc_state.coord_system[Y_AXIS] + gc_state.coord_offset[Y_AXIS]; - position[Z_AXIS] = calc_fwd[Z_AXIS] - gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]; - } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "MSG:Fwd Kin Error"); - } -} - -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 -} +// 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() { #ifdef USE_CUSTOM_HOMING @@ -407,9 +372,7 @@ void kinematics_post_homing() { 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); + motors_to_cartesian(last_cartesian, last_angle, 3); // grbl_msg_sendf(CLIENT_SERIAL, // MsgLevel::Info, diff --git a/Grbl_Esp32/Custom/polar_coaster.cpp b/Grbl_Esp32/Custom/polar_coaster.cpp index 85cf3f8a..7041fbb0 100644 --- a/Grbl_Esp32/Custom/polar_coaster.cpp +++ b/Grbl_Esp32/Custom/polar_coaster.cpp @@ -85,9 +85,8 @@ void kinematics_post_homing() { */ -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) { - //static float last_angle = 0; - //static float last_radius = 0; + +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { float dx, dy, dz; // distances in each cartesian axis float p_dx, p_dy, p_dz; // distances in each polar axis float dist, polar_dist; // the distances in both systems...used to determine feed rate @@ -139,11 +138,19 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // end determining new feed rate polar[RADIUS_AXIS] += x_offset; polar[Z_AXIS] += z_offset; + + // mc_line() returns false if a jog is cancelled. + // In that case we stop sending segments to the planner. + if (!mc_line(polar, pl_data)) { + return false; + } + + // last_radius = polar[RADIUS_AXIS]; last_angle = polar[POLAR_AXIS]; - mc_line(polar, pl_data); } // TO DO don't need a feedrate for rapids + return true; } /* @@ -159,18 +166,10 @@ position = the current machine position converted = position with forward kinematics applied. */ -void forward_kinematics(float* position) { - float original_position[N_AXIS]; // temporary storage of original - float print_position[N_AXIS]; - int32_t current_position[N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - original_position[X_AXIS] = print_position[X_AXIS] - gc_state.coord_system[X_AXIS] + gc_state.coord_offset[X_AXIS]; - original_position[Y_AXIS] = print_position[Y_AXIS] - gc_state.coord_system[Y_AXIS] + gc_state.coord_offset[Y_AXIS]; - original_position[Z_AXIS] = print_position[Z_AXIS] - gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]; - position[X_AXIS] = cos(radians(original_position[Y_AXIS])) * original_position[X_AXIS] * -1; - position[Y_AXIS] = sin(radians(original_position[Y_AXIS])) * original_position[X_AXIS]; - position[Z_AXIS] = original_position[Z_AXIS]; // unchanged +void motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + cartesian[X_AXIS] = cos(radians(motors[Y_AXIS])) * motors[X_AXIS] * -1; + cartesian[Y_AXIS] = sin(radians(motors[Y_AXIS])) * motors[X_AXIS]; + cartesian[Z_AXIS] = motors[Z_AXIS]; // unchanged } // helper functions diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp index c18ef200..70bb88d2 100644 --- a/Grbl_Esp32/src/Error.cpp +++ b/Grbl_Esp32/src/Error.cpp @@ -80,4 +80,5 @@ std::map ErrorNames = { { Error::NvsGetStatsFailed, "Failed to get setting status" }, { Error::AuthenticationFailed, "Authentication failed!" }, { Error::AnotherInterfaceBusy, "Another interface is busy" }, + { Error::JogCancelled, "Jog Cancelled" }, }; diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h index 14b443eb..6b8fe3e0 100644 --- a/Grbl_Esp32/src/Error.h +++ b/Grbl_Esp32/src/Error.h @@ -84,6 +84,7 @@ enum class Error : uint8_t { AuthenticationFailed = 110, Eol = 111, AnotherInterfaceBusy = 120, + JogCancelled = 130, }; extern std::map ErrorNames; diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index bd1bf2b9..d55eb1ed 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -494,9 +494,7 @@ Error gc_execute_line(char* line, uint8_t client) { break; case 6: // tool change gc_block.modal.tool_change = ToolChange::Enable; -#ifdef USE_TOOL_CHANGE //user_tool_change(gc_state.tool); -#endif mg_word_bit = ModalGroup::MM6; break; case 7: @@ -1287,14 +1285,16 @@ Error gc_execute_line(char* line, uint8_t client) { FAIL(Error::InvalidJogCommand); } // Initialize planner data to current spindle and coolant modal state. - pl_data->spindle_speed = gc_state.spindle_speed; - pl_data->spindle = gc_state.modal.spindle; - pl_data->coolant = gc_state.modal.coolant; - Error status = jog_execute(pl_data, &gc_block); - if (status == Error::Ok) { + pl_data->spindle_speed = gc_state.spindle_speed; + pl_data->spindle = gc_state.modal.spindle; + pl_data->coolant = gc_state.modal.coolant; + bool cancelledInflight = false; + Error status = jog_execute(pl_data, &gc_block, &cancelledInflight); + if (status == Error::Ok && !cancelledInflight) { memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); } - return status; + // JogCancelled is not reported as a GCode error + return status == Error::JogCancelled ? Error::Ok : status; } // If in laser mode, setup laser power based on current and past parser conditions. if (spindle->inLaserMode()) { @@ -1361,9 +1361,7 @@ Error gc_execute_line(char* line, uint8_t client) { // gc_state.tool = gc_block.values.t; // [6. Change tool ]: NOT SUPPORTED if (gc_block.modal.tool_change == ToolChange::Enable) { -#ifdef USE_TOOL_CHANGE user_tool_change(gc_state.tool); -#endif } // [7. Spindle control ]: if (gc_state.modal.spindle != gc_block.modal.spindle) { @@ -1485,9 +1483,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_kins(gc_block.values.xyz, pl_data, gc_state.position); + cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } - mc_line_kins(coord_data, pl_data, gc_state.position); + cartesian_to_motors(coord_data, pl_data, gc_state.position); memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); break; case NonModal::SetHome0: @@ -1515,12 +1513,10 @@ Error gc_execute_line(char* line, uint8_t client) { if (axis_command == AxisCommand::MotionMode) { GCUpdatePos gc_update_pos = GCUpdatePos::Target; if (gc_state.modal.motion == Motion::Linear) { - //mc_line(gc_block.values.xyz, pl_data); - mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); + cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } else if (gc_state.modal.motion == Motion::Seek) { pl_data->motion.rapidMotion = 1; // Set rapid motion flag. - //mc_line(gc_block.values.xyz, pl_data); - mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); + cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } else if ((gc_state.modal.motion == Motion::CwArc) || (gc_state.modal.motion == Motion::CcwArc)) { mc_arc(gc_block.values.xyz, pl_data, @@ -1604,9 +1600,7 @@ Error gc_execute_line(char* line, uint8_t client) { coolant_off(); } report_feedback_message(Message::ProgramEnd); -#ifdef USE_M30 user_m30(); -#endif break; } gc_state.modal.program_flow = ProgramFlow::Running; // Reset program flow. diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index a553621c..ea0bc2a3 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -105,6 +105,10 @@ static void reset_variables() { plan_sync_position(); gc_sync_position(); report_init_message(CLIENT_ALL); + + // used to keep track of a jog command sent to mc_line() so we can cancel it. + // this is needed if a jogCancel comes along after we have already parsed a jog and it is in-flight. + sys_pl_data_inflight = NULL; } void run_once() { @@ -118,6 +122,10 @@ void run_once() { void __attribute__((weak)) machine_init() {} void __attribute__((weak)) display_init() {} + +void __attribute__((weak)) user_m30() {} + +void __attribute__((weak)) user_tool_change(uint8_t new_tool) {} /* setup() and loop() in the Arduino .ino implements this control flow: diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 3f949aae..f0dbd60d 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210419"; +const char* const GRBL_VERSION_BUILD = "20210424"; //#include #include @@ -93,27 +93,21 @@ const char* const GRBL_VERSION_BUILD = "20210419"; void grbl_init(); void run_once(); -void machine_init(); // weak definition in Grbl.cpp -void display_init(); // weak definition in Grbl.cpp +void machine_init(); // weak definition in Grbl.cpp +void display_init(); // weak definition in Grbl.cpp +void user_m30(); // weak definition in Grbl.cpp/ +void user_tool_change(uint8_t new_tool); // weak definition in Grbl.cpp bool user_defined_homing(uint8_t cycle_mask); // weak definition in Limits.cpp -// Called if USE_KINEMATICS is defined +// weak definitions in MotionControl.cpp +bool cartesian_to_motors(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); +bool limitsCheckTravel(float* target); // weak in Limits.cpp; true if out of range -// Called if USE_FWD_KINEMATICS is defined -void inverse_kinematics(float* position); // used to return a converted value -void forward_kinematics(float* position); // weak definition in Report.cpp +void motors_to_cartesian(float* cartestian, float* motors, int n_axis); // weak definition // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined void user_defined_macro(uint8_t index); - -// Called if USE_M30 is defined -void user_m30(); - -// Called if USE_TOOL_CHANGE is defined -void user_tool_change(uint8_t new_tool); diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp index 1c0c3d00..03474c7a 100644 --- a/Grbl_Esp32/src/Jog.cpp +++ b/Grbl_Esp32/src/Jog.cpp @@ -24,11 +24,13 @@ #include "Grbl.h" // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. -Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { +// cancelledInflight will be set to true if was not added to parser due to a cancelJog. +Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* cancelledInflight) { // Initialize planner data struct for jogging motions. // NOTE: Spindle and coolant are allowed to fully function with overrides during a jog. pl_data->feed_rate = gc_block->values.f; pl_data->motion.noFeedOverride = 1; + pl_data->is_jog = true; #ifdef USE_LINE_NUMBERS pl_data->line_number = gc_block->values.n; #endif @@ -37,12 +39,10 @@ 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. -#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 + // Valid jog command. Plan, set state, and execute. + if (!cartesian_to_motors(gc_block->values.xyz, pl_data, gc_state.position)) { + return Error::JogCancelled; + } if (sys.state == State::Idle) { if (plan_get_current_block() != NULL) { // Check if there is a block to execute. diff --git a/Grbl_Esp32/src/Jog.h b/Grbl_Esp32/src/Jog.h index 59307834..1867871e 100644 --- a/Grbl_Esp32/src/Jog.h +++ b/Grbl_Esp32/src/Jog.h @@ -28,4 +28,5 @@ const int JOG_LINE_NUMBER = 0; // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. -Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block); +// cancelledInflight will be set to true if was not added to parser due to a cancelJog. +Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block, bool* cancelledInflight); diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index cf184eb7..fec18cfd 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -104,7 +104,6 @@ void limits_go_home(uint8_t cycle_mask) { // Initialize variables used for homing computations. uint8_t n_cycle = (2 * n_homing_locate_cycle + 1); uint8_t step_pin[MAX_N_AXIS]; - float target[MAX_N_AXIS]; float max_travel = 0.0; auto n_axis = number_axis->get(); @@ -122,7 +121,7 @@ void limits_go_home(uint8_t cycle_mask) { uint8_t n_active_axis; AxisMask limit_state, axislock; do { - system_convert_array_steps_to_mpos(target, sys_position); + float* target = system_get_mpos(); // Initialize and declare variables needed for homing routine. axislock = 0; n_active_axis = 0; @@ -397,7 +396,7 @@ float limitsMinPosition(uint8_t axis) { // Checks and reports if target array exceeds machine travel limits. // Return true if exceeding limits // Set $/MaxTravel=0 to selectively remove an axis from soft limit checks -bool limitsCheckTravel(float* target) { +bool __attribute__((weak)) limitsCheckTravel(float* target) { uint8_t idx; auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { diff --git a/Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h b/Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h new file mode 100644 index 00000000..fad84751 --- /dev/null +++ b/Grbl_Esp32/src/Machines/6_pack_TMC2130_XYZ_Test.h @@ -0,0 +1,110 @@ +#pragma once +// clang-format off + +/* + 6_pack_TMC2130_XYZ_PWM.h + + Part of Grbl_ESP32 + Pin assignments for the ESP32 SPI 6-axis board + + 2021-0302 B. Dring for Mike T. + + 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 "6 Pack TMC2130 XYZ PWM" + +#define N_AXIS 3 + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +#define USE_I2S_STEPS +//#define DEFAULT_STEPPER ST_I2S_STATIC + +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 + +#define TRINAMIC_RUN_MODE Motors::TrinamicMode::CoolStep +#define TRINAMIC_HOMING_MODE Motors::TrinamicMode::CoolStep + +// 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 + + +/* + Socket I/O reference + The list of modules is here... + https://github.com/bdring/6-Pack_CNC_Controller/wiki/CNC-I-O-Module-List + Click on each module to get example for using the modules in the sockets + +Socket #1 +#1 GPIO_NUM_33 (Sg1) +#2 GPIO_NUM_32 (Sg2) +#3 GPIO_NUM_35 (Sg3) (input only) +#4 GPIO_NUM_34 (Sg4) (input only) + +Socket #2 +#1 GPIO_NUM_2 +#2 GPIO_NUM_25 +#3 GPIO_NUM_39 (Sg5) (input only) +#4 GPIO_NUM_36 (Sg6) (input only) + +Socket #3 +#1 GPIO_NUM_26 +#2 GPIO_NUM_4 +#3 GPIO_NUM_16 +#4 GPIO_NUM_27 + +Socket #4 +#1 GPIO_NUM_14 +#2 GPIO_NUM_13 +#3 GPIO_NUM_15 +#4 GPIO_NUM_12 + +Socket #5 +#1 I2SO(24) (output only) +#2 I2SO(25) (output only) +#3 I2SO26) (output only) + +*/ + +// Socket #1 (Empty) +// Install StallGuard Jumpers +#define X_LIMIT_PIN GPIO_NUM_33 // Sg1 +#define Y_LIMIT_PIN GPIO_NUM_32 // Sg2 +#define Z_LIMIT_PIN GPIO_NUM_35 // Sg3 +#define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_34 // Sg4 + + +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/src/Machines/atari_1020.h b/Grbl_Esp32/src/Machines/atari_1020.h index eecc2146..50186bb8 100644 --- a/Grbl_Esp32/src/Machines/atari_1020.h +++ b/Grbl_Esp32/src/Machines/atari_1020.h @@ -140,11 +140,7 @@ #define ATARI_HOMING_ATTEMPTS 13 // tells grbl we have some special functions to call -#define USE_MACHINE_INIT -#define USE_CUSTOM_HOMING -#define USE_TOOL_CHANGE #define ATARI_TOOL_CHANGE_Z 5.0 -#define USE_M30 // use the user defined end of program #ifndef atari_h #define atari_h diff --git a/Grbl_Esp32/src/Machines/fysetc_ant.h b/Grbl_Esp32/src/Machines/fysetc_ant.h new file mode 100644 index 00000000..79607088 --- /dev/null +++ b/Grbl_Esp32/src/Machines/fysetc_ant.h @@ -0,0 +1,85 @@ +#pragma once +// clang-format off + +/* + fysetc_ant.h + https://github.com/FYSETC/FYSETC-E4 + + 2020-12-03 B. Dring + + This is a machine definition file to use the FYSETC E4 3D Printer controller + This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used. + There are 5 inputs + The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to + spindle, mist and flood coolant to drive an external relay. + + 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 "FYSETC E4 3D Printer Controller" + +#define N_AXIS 4 + +#define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp" + +#define USE_KINEMATICS // there are kinematic equations for this machine + +#define TRINAMIC_RUN_MODE TrinamicMode :: StealthChop +#define TRINAMIC_HOMING_MODE TrinamicMode :: StealthChop + +#define TMC_UART UART_NUM_1 +#define TMC_UART_RX GPIO_NUM_21 +#define TMC_UART_TX GPIO_NUM_22 + +#define X_TRINAMIC_DRIVER 2209 +#define X_STEP_PIN GPIO_NUM_27 +#define X_DIRECTION_PIN GPIO_NUM_26 +#define X_RSENSE TMC2209_RSENSE_DEFAULT +#define X_DRIVER_ADDRESS 1 +#define DEFAULT_X_MICROSTEPS 16 + +#define Y_TRINAMIC_DRIVER 2209 +#define Y_STEP_PIN GPIO_NUM_33 +#define Y_DIRECTION_PIN GPIO_NUM_32 +#define Y_RSENSE TMC2209_RSENSE_DEFAULT +#define Y_DRIVER_ADDRESS 3 +#define DEFAULT_Y_MICROSTEPS 16 + +#define Z_TRINAMIC_DRIVER 2209 +#define Z_STEP_PIN GPIO_NUM_14 +#define Z_DIRECTION_PIN GPIO_NUM_12 +#define Z_RSENSE TMC2209_RSENSE_DEFAULT +#define Z_DRIVER_ADDRESS 0 +#define DEFAULT_Z_MICROSTEPS 16 + +#define A_TRINAMIC_DRIVER 2209 +#define A_STEP_PIN GPIO_NUM_16 +#define A_DIRECTION_PIN GPIO_NUM_17 +#define A_RSENSE TMC2209_RSENSE_DEFAULT +#define A_DRIVER_ADDRESS 2 +#define DEFAULT_A_MICROSTEPS 16 + +#define X_LIMIT_PIN GPIO_NUM_34 +#define Y_LIMIT_PIN GPIO_NUM_35 +#define Z_LIMIT_PIN GPIO_NUM_15 +#define A_LIMIT_PIN GPIO_NUM_36 // Labeled TB +#define PROBE_PIN GPIO_NUM_39 // Labeled TE + +// OK to comment out to use pin for other features +#define STEPPERS_DISABLE_PIN GPIO_NUM_25 + +#define SPINDLE_TYPE SpindleType::RELAY +#define SPINDLE_OUTPUT_PIN GPIO_NUM_13 // labeled Fan +#define COOLANT_MIST_PIN GPIO_NUM_2 // Labeled Hotbed +#define COOLANT_FLOOD_PIN GPIO_NUM_4 // Labeled Heater diff --git a/Grbl_Esp32/src/Machines/fystec_e4.h b/Grbl_Esp32/src/Machines/fysetc_e4.h similarity index 95% rename from Grbl_Esp32/src/Machines/fystec_e4.h rename to Grbl_Esp32/src/Machines/fysetc_e4.h index 63b47bab..1bb8800d 100644 --- a/Grbl_Esp32/src/Machines/fystec_e4.h +++ b/Grbl_Esp32/src/Machines/fysetc_e4.h @@ -2,12 +2,12 @@ // clang-format off /* - fystec_e4.h + fysetc_e4.h https://github.com/FYSETC/FYSETC-E4 2020-12-03 B. Dring - This is a machine definition file to use the FYSTEC E4 3D Printer controller + This is a machine definition file to use the FYSETC E4 3D Printer controller This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used. There are 5 inputs The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to @@ -27,7 +27,7 @@ along with Grbl_ESP32. If not, see . */ -#define MACHINE_NAME "FYSTEC E4 3D Printer Controller" +#define MACHINE_NAME "FYSETC E4 3D Printer Controller" #define N_AXIS 4 diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h index 41e35c2a..a6bd8438 100644 --- a/Grbl_Esp32/src/Machines/midtbot.h +++ b/Grbl_Esp32/src/Machines/midtbot.h @@ -29,11 +29,7 @@ #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 MIDTBOT // applies the midTbot geometry correction to the CoreXY kinematics #define SPINDLE_TYPE SpindleType::NONE @@ -50,8 +46,6 @@ #define Z_SERVO_PIN GPIO_NUM_27 -// Set $Homing/Cycle0=Y and $Homing/Cycle1=X - #define SPINDLE_TYPE SpindleType::NONE // defaults diff --git a/Grbl_Esp32/src/Machines/midtbot_x2.h b/Grbl_Esp32/src/Machines/midtbot_x2.h new file mode 100644 index 00000000..9e904133 --- /dev/null +++ b/Grbl_Esp32/src/Machines/midtbot_x2.h @@ -0,0 +1,119 @@ +#pragma once +// clang-format off + +/* + midtbot.h + Part of Grbl_ESP32 + + Pin assignments for the Buildlog.net midtbot + https://github.com/bdring/midTbot_esp32 + + 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 "midTbot" + +#define DISPLAY_CODE_FILENAME "Custom/oled_basic.cpp" + +#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 SPINDLE_TYPE SpindleType::NONE + +#define TRINAMIC_UART_RUN_MODE TrinamicUartMode :: StealthChop +#define TRINAMIC_UART_HOMING_MODE TrinamicUartMode :: StealthChop + +#define TMC_UART UART_NUM_1 +#define TMC_UART_RX GPIO_NUM_21 +#define TMC_UART_TX GPIO_NUM_22 + +#define X_TRINAMIC_DRIVER 2209 +#define X_STEP_PIN GPIO_NUM_25 //GPIO_NUM_32 +#define X_DIRECTION_PIN GPIO_NUM_33 // GPIO_NUM_26 +#define X_RSENSE TMC2209_RSENSE_DEFAULT +#define X_DRIVER_ADDRESS 1 +#define DEFAULT_X_MICROSTEPS 16 + +#define Y_TRINAMIC_DRIVER 2209 +#define Y_STEP_PIN GPIO_NUM_32 //GPIO_NUM_25 +#define Y_DIRECTION_PIN GPIO_NUM_26 //GPIO_NUM_33 +#define Y_RSENSE TMC2209_RSENSE_DEFAULT +#define Y_DRIVER_ADDRESS 0 +#define DEFAULT_Y_MICROSTEPS 16 + +#define STEPPERS_DISABLE_PIN GPIO_NUM_27 + +#define X_LIMIT_PIN GPIO_NUM_4 +#define Y_LIMIT_PIN GPIO_NUM_12 + +#define Z_SERVO_PIN GPIO_NUM_15 + +// Set $Homing/Cycle0=Y and $Homing/Cycle1=X + +#define SPINDLE_TYPE SpindleType::NONE + +// ================== defaults ================================ +#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 + +#define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t +#define DEFAULT_DIRECTION_INVERT_MASK 2 // uint8_t +#define DEFAULT_INVERT_ST_ENABLE 0 // boolean +#define DEFAULT_INVERT_LIMIT_PINS 0 // boolean +#define DEFAULT_INVERT_PROBE_PIN 0 // boolean + +#define DEFAULT_STATUS_REPORT_MASK 1 + +#define DEFAULT_JUNCTION_DEVIATION 0.01 // mm +#define DEFAULT_ARC_TOLERANCE 0.002 // mm +#define DEFAULT_REPORT_INCHES 0 // false + +#define DEFAULT_SOFT_LIMIT_ENABLE 0 // false +#define DEFAULT_HARD_LIMIT_ENABLE 0 // false + +#define DEFAULT_HOMING_ENABLE 1 +#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 100.0 +#define DEFAULT_Y_STEPS_PER_MM 100.0 +#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode + +#define DEFAULT_X_MAX_RATE 8000.0 // mm/min +#define DEFAULT_Y_MAX_RATE 8000.0 // mm/min +#define DEFAULT_Z_MAX_RATE 5000.0 // mm/min + +#define DEFAULT_X_ACCELERATION 200.0 // mm/sec^2. 200 mm/sec^2 = 720000 mm/min^2 +#define DEFAULT_Y_ACCELERATION 200.0 // mm/sec^2 +#define DEFAULT_Z_ACCELERATION 100.0 // mm/sec^2 + +#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 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/mpcnc_laser_module_v1p2.h b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h index 43f2864c..a9fecf73 100644 --- a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h +++ b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h @@ -30,7 +30,6 @@ #define MACHINE_NAME "MPCNC_V1P2 with Laser Module" // The laser module fires without a low signal. This keeps the enable on -#define USE_MACHINE_INIT #define LVL_SHIFT_ENABLE GPIO_NUM_32 #define CUSTOM_CODE_FILENAME "Custom/mpcnc_laser_module.cpp" diff --git a/Grbl_Esp32/src/Machines/polar_coaster.h b/Grbl_Esp32/src/Machines/polar_coaster.h index b0e45e7c..03f9f1ea 100644 --- a/Grbl_Esp32/src/Machines/polar_coaster.h +++ b/Grbl_Esp32/src/Machines/polar_coaster.h @@ -37,9 +37,6 @@ #define POLAR_AXIS 1 #define SEGMENT_LENGTH 0.5 // segment length in mm -#define USE_KINEMATICS -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_M30 #define X_STEP_PIN GPIO_NUM_15 #define Y_STEP_PIN GPIO_NUM_2 @@ -123,4 +120,6 @@ #define DEFAULT_X_MAX_TRAVEL 50.0 // mm NOTE: Must be a positive value. #define DEFAULT_Y_MAX_TRAVEL 300.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_Z_HOMING_MPOS DEFAULT_Z_MAX_TRAVEL // stays up after homing diff --git a/Grbl_Esp32/src/Machines/tapster_3.h b/Grbl_Esp32/src/Machines/tapster_3.h index b4de2b1c..d801aed5 100644 --- a/Grbl_Esp32/src/Machines/tapster_3.h +++ b/Grbl_Esp32/src/Machines/tapster_3.h @@ -23,12 +23,6 @@ #define N_AXIS 3 -// ================= 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 - // ================== Delta Geometry =========================== #define RADIUS_FIXED 70.0 // radius of the fixed side (length of motor cranks) diff --git a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h index 6f88a816..6b4047ee 100644 --- a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h +++ b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h @@ -26,8 +26,6 @@ #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 @@ -41,10 +39,6 @@ #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) diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 1f2dc53f..00762b97 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -33,15 +33,6 @@ SquaringMode ganged_mode = SquaringMode::Dual; -// this allows kinematics to be used. -void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position) { -#ifndef USE_KINEMATICS - mc_line(target, pl_data); -#else // else use kinematics - inverse_kinematics(target, pl_data, position); -#endif -} - // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. @@ -49,7 +40,11 @@ void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position) { // segments, must pass through this routine before being passed to the planner. The seperation of // mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being // in the planner and to let backlash compensation or canned cycle integration simple and direct. -void mc_line(float* target, plan_line_data_t* pl_data) { +// returns true if line was submitted to planner, or false if intentionally dropped. +bool mc_line(float* target, plan_line_data_t* pl_data) { + bool submitted_result = false; + // store the plan data so it can be cancelled by the protocol system if needed + sys_pl_data_inflight = pl_data; // If enabled, check for soft limit violations. Placed here all line motions are picked up // from everywhere in Grbl. if (soft_limits->get()) { @@ -60,7 +55,8 @@ void mc_line(float* target, plan_line_data_t* pl_data) { } // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. if (sys.state == State::CheckMode) { - return; + sys_pl_data_inflight = NULL; + return submitted_result; } // NOTE: Backlash compensation may be installed here. It will need direction info to track when // to insert a backlash line motion(s) before the intended line motion and will require its own @@ -80,7 +76,8 @@ void mc_line(float* target, plan_line_data_t* pl_data) { do { protocol_execute_realtime(); // Check for any run-time commands if (sys.abort) { - return; // Bail, if system abort. + sys_pl_data_inflight = NULL; + return submitted_result; // Bail, if system abort. } if (plan_check_full_buffer()) { protocol_auto_cycle_start(); // Auto-cycle start when buffer is full. @@ -90,9 +87,29 @@ void mc_line(float* target, plan_line_data_t* pl_data) { } while (1); // Plan and queue motion into planner buffer // uint8_t plan_status; // Not used in normal operation. - plan_buffer_line(target, pl_data); + if (sys_pl_data_inflight == pl_data) { + plan_buffer_line(target, pl_data); + submitted_result = true; + } + sys_pl_data_inflight = NULL; + return submitted_result; } +bool __attribute__((weak)) cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { + return mc_line(target, pl_data); +} + +bool __attribute__((weak)) kinematics_pre_homing(uint8_t cycle_mask) { + return false; // finish normal homing cycle +} + +void __attribute__((weak)) kinematics_post_homing() {} + +void __attribute__((weak)) motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + memcpy(cartesian, motors, n_axis * sizeof(motors[0])); +} + +void __attribute__((weak)) forward_kinematics(float* position) {} // Execute an arc in offset mode format. position == current xyz, target == target xyz, // offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is // the direction of helical travel, radius == circle radius, isclockwise boolean. Used @@ -117,14 +134,12 @@ void mc_arc(float* target, 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 uint16_t n; auto n_axis = number_axis->get(); for (n = 0; n < n_axis; n++) { previous_position[n] = position[n]; } -#endif // CCW angle between position and target from circle center. Only one atan2() trig computation required. float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); if (is_clockwise_arc) { // Correct atan2 output per direction @@ -206,15 +221,11 @@ void mc_arc(float* target, position[axis_0] = center_axis0 + r_axis0; position[axis_1] = center_axis1 + r_axis1; position[axis_linear] += linear_per_segment; -#ifdef USE_KINEMATICS pl_data->feed_rate = original_feedrate; // This restores the feedrate kinematics may have altered - mc_line_kins(position, pl_data, previous_position); + cartesian_to_motors(position, pl_data, previous_position); previous_position[axis_0] = position[axis_0]; previous_position[axis_1] = position[axis_1]; previous_position[axis_linear] = position[axis_linear]; -#else - mc_line(position, pl_data); -#endif // Bail mid-circle on system abort. Runtime command check already performed by mc_line. if (sys.abort) { return; @@ -222,7 +233,7 @@ void mc_arc(float* target, } } // Ensure last segment arrives at target location. - mc_line_kins(target, pl_data, previous_position); + cartesian_to_motors(target, pl_data, previous_position); } // Execute dwell in seconds. @@ -292,11 +303,9 @@ void mc_homing_cycle(uint8_t cycle_mask) { // This give kinematics a chance to do something before normal homing // if it returns true, the homing is canceled. -#ifdef USE_KINEMATICS if (kinematics_pre_homing(cycle_mask)) { return; } -#endif // Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems // with machines with limits wired on both ends of travel to one limit pin. // TODO: Move the pin-specific LIMIT_PIN call to Limits.cpp as a function. @@ -366,10 +375,8 @@ void mc_homing_cycle(uint8_t cycle_mask) { // Sync gcode parser and planner positions to homed position. gc_sync_position(); plan_sync_position(); -#ifdef USE_KINEMATICS // This give kinematics a chance to do something after normal homing kinematics_post_homing(); -#endif // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle. limits_init(); } @@ -412,7 +419,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par } // Setup and queue probing motion. Auto cycle-start should not start the cycle. grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Found"); - mc_line_kins(target, pl_data, gc_state.position); + cartesian_to_motors(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/MotionControl.h b/Grbl_Esp32/src/MotionControl.h index c7adebc5..c9f26c04 100644 --- a/Grbl_Esp32/src/MotionControl.h +++ b/Grbl_Esp32/src/MotionControl.h @@ -35,8 +35,8 @@ const int PARKING_MOTION_LINE_NUMBER = 0; // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. -void mc_line_kins(float* target, plan_line_data_t* pl_data, float* position); -void mc_line(float* target, plan_line_data_t* pl_data); +bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position); +bool mc_line(float* target, plan_line_data_t* pl_data); // returns true if line was submitted to planner // Execute an arc in offset mode format. position == current xyz, target == target xyz, // offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.cpp b/Grbl_Esp32/src/Motors/Dynamixel2.cpp index 3c3faedc..8543dac2 100644 --- a/Grbl_Esp32/src/Motors/Dynamixel2.cpp +++ b/Grbl_Esp32/src/Motors/Dynamixel2.cpp @@ -36,7 +36,7 @@ namespace Motors { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dynamixel Error. Missing pin definitions"); _has_errors = true; } else { - _has_errors = false; // The motor can be used + _has_errors = false; // The motor can be used } } @@ -103,8 +103,8 @@ namespace Motors { } } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(_axis_index, _dual_axis_index), -_id); + grbl_msg_sendf( + CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(_axis_index, _dual_axis_index), _id); return false; } @@ -197,7 +197,7 @@ _id); set_disable(false); set_location(); // force the PWM to update now - return false; // Cannot do conventional homing + return false; // Cannot do conventional homing } void Dynamixel2::dxl_goal_position(int32_t position) { @@ -345,16 +345,14 @@ _id); tx_message[++msg_index] = 4; // low order data length tx_message[++msg_index] = 0; // high order data length - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); + float* mpos = system_get_mpos(); for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { current_id = ids[axis][gang_index]; if (current_id != 0) { count++; // keep track of the count for the message length - //determine the location of the axis - float target = system_convert_axis_steps_to_mpos(sys_position, axis); // get the axis machine position in mm - dxl_count_min = DXL_COUNT_MIN; dxl_count_max = DXL_COUNT_MAX; @@ -362,7 +360,8 @@ _id); swap(dxl_count_min, dxl_count_max); // map the mm range to the servo range - dxl_position = (uint32_t)mapConstrain(target, limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max); + dxl_position = + (uint32_t)mapConstrain(mpos[axis], limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max); tx_message[++msg_index] = current_id; // ID of the servo tx_message[++msg_index] = dxl_position & 0xFF; // data diff --git a/Grbl_Esp32/src/Motors/Motor.cpp b/Grbl_Esp32/src/Motors/Motor.cpp index 195601f8..6de1c869 100644 --- a/Grbl_Esp32/src/Motors/Motor.cpp +++ b/Grbl_Esp32/src/Motors/Motor.cpp @@ -16,7 +16,6 @@ Make sure public/private/protected is cleaned up. Only a few Unipolar axes have been setup in init() Get rid of Z_SERVO, just reply on Z_SERVO_PIN - Deal with custom machine ... machine_trinamic_setup(); Class is ready to deal with non SPI pins, but they have not been needed yet. It would be nice in the config message though Testing @@ -34,8 +33,7 @@ #include "Motor.h" namespace Motors { - Motor::Motor(uint8_t axis_index) : - _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {} + Motor::Motor(uint8_t axis_index) : _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {} void Motor::debug_message() {} diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index 4cb5289a..21f955bd 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -16,7 +16,6 @@ Make sure public/private/protected is cleaned up. Only a few Unipolar axes have been setup in init() Get rid of Z_SERVO, just reply on Z_SERVO_PIN - Deal with custom machine ... machine_trinamic_setup(); Class is ready to deal with non SPI pins, but they have not been needed yet. It would be nice in the config message though Testing diff --git a/Grbl_Esp32/src/Planner.h b/Grbl_Esp32/src/Planner.h index 1f5f3eb4..812229c9 100644 --- a/Grbl_Esp32/src/Planner.h +++ b/Grbl_Esp32/src/Planner.h @@ -91,6 +91,7 @@ typedef struct { #ifdef USE_LINE_NUMBERS int32_t line_number; // Desired line number to report when executing. #endif + bool is_jog; // true if this was generated due to a jog command } plan_line_data_t; // Initialize and reset the motion plan subsystem diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index 4d055e51..0119a153 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -532,7 +532,6 @@ static void protocol_exec_rt_suspend() { #ifdef PARKING_ENABLE // Declare and initialize parking local variables float restore_target[MAX_N_AXIS]; - float parking_target[MAX_N_AXIS]; float retract_waypoint = PARKING_PULLOUT_INCREMENT; plan_line_data_t plan_data; plan_line_data_t* pl_data = &plan_data; @@ -567,12 +566,21 @@ static void protocol_exec_rt_suspend() { if (sys.abort) { return; } + // if a jogCancel comes in and we have a jog "in-flight" (parsed and handed over to mc_line()), + // then we need to cancel it before it reaches the planner. otherwise we may try to move way out of + // normal bounds, especially with senders that issue a series of jog commands before sending a cancel. + if (sys.suspend.bit.jogCancel && sys_pl_data_inflight != NULL && ((plan_line_data_t*)sys_pl_data_inflight)->is_jog) { + sys_pl_data_inflight = NULL; + } // Block until initial hold is complete and the machine has stopped motion. if (sys.suspend.bit.holdComplete) { // Parking manager. Handles de/re-energizing, switch state checks, and parking motions for // the safety door and sleep states. if (sys.state == State::SafetyDoor || sys.state == State::Sleep) { // Handles retraction motions and de-energizing. +#ifdef PARKING_ENABLE + float* parking_target = system_get_mpos(); +#endif if (!sys.suspend.bit.retractComplete) { // Ensure any prior spindle stop override is disabled at start of safety door routine. sys.spindle_stop_ovr.value = 0; // Disable override @@ -581,9 +589,8 @@ static void protocol_exec_rt_suspend() { coolant_off(); #else // Get current position and store restore location and spindle retract waypoint. - system_convert_array_steps_to_mpos(parking_target, sys_position); if (!sys.suspend.bit.restartRetract) { - memcpy(restore_target, parking_target, sizeof(parking_target)); + memcpy(restore_target, parking_target, sizeof(restore_target[0]) * number_axis->get()); retract_waypoint += restore_target[PARKING_AXIS]; retract_waypoint = MIN(retract_waypoint, PARKING_TARGET); } diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 3339a23c..9afe1c45 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -299,11 +299,11 @@ void report_grbl_help(uint8_t client) { // These values are retained until Grbl is power-cycled, whereby they will be re-zeroed. void report_probe_parameters(uint8_t client) { // Report in terms of machine position. - float print_position[MAX_N_AXIS]; - char probe_rpt[(axesStringLen + 13 + 6 + 1)]; // the probe report we are building here - char temp[axesStringLen]; + char probe_rpt[(axesStringLen + 13 + 6 + 1)]; // the probe report we are building here + char temp[axesStringLen]; strcpy(probe_rpt, "[PRB:"); // initialize the string with the first characters // get the machine position and put them into a string and append to the probe report + float print_position[MAX_N_AXIS]; system_convert_array_steps_to_mpos(print_position, sys_probe_position); report_util_axis_values(print_position, temp); strcat(probe_rpt, temp); @@ -574,28 +574,6 @@ void report_echo_line_received(char* line, uint8_t client) { // float print_position = returned position // float wco = returns the work coordinate offset // bool wpos = true for work position compensation -void report_calc_status_position(float* print_position, float* wco, bool wpos) { - int32_t current_position[MAX_N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - - //float wco[MAX_N_AXIS]; - if (wpos || (sys.report_wco_counter == 0)) { - auto n_axis = number_axis->get(); - for (uint8_t idx = 0; idx < n_axis; idx++) { - // Apply work coordinate offsets and tool length offset to current position. - wco[idx] = gc_state.coord_system[idx] + gc_state.coord_offset[idx]; - if (idx == TOOL_LENGTH_OFFSET_AXIS) { - wco[idx] += gc_state.tool_length_offset; - } - if (wpos) { - print_position[idx] -= wco[idx]; - } - } - } - - forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version -} // Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram // and the actual location of the CNC machine. Users may change the following function to their @@ -603,20 +581,19 @@ void report_calc_status_position(float* print_position, float* wco, bool wpos) { // requires as it minimizes the computational overhead and allows grbl to keep running smoothly, // especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz). void report_realtime_status(uint8_t client) { - float print_position[MAX_N_AXIS]; - char status[200]; - char temp[MAX_N_AXIS * 20]; + char status[200]; + char temp[MAX_N_AXIS * 20]; strcpy(status, "<"); strcat(status, report_state_text()); // Report position + float* print_position = system_get_mpos(); if (bit_istrue(status_mask->get(), RtStatus::Position)) { - calc_mpos(print_position); strcat(status, "|MPos:"); } else { - calc_wpos(print_position); strcat(status, "|WPos:"); + mpos_to_wpos(print_position); } report_util_axis_values(print_position, temp); strcat(status, temp); @@ -956,26 +933,14 @@ void reportTaskStackSize(UBaseType_t& saved) { #endif } -void calc_mpos(float* print_position) { - int32_t current_position[MAX_N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version -} - -void calc_wpos(float* print_position) { - int32_t current_position[MAX_N_AXIS]; // Copy current state of the system position variable - memcpy(current_position, sys_position, sizeof(sys_position)); - system_convert_array_steps_to_mpos(print_position, current_position); - +void mpos_to_wpos(float* position) { float* wco = get_wco(); auto n_axis = number_axis->get(); for (int idx = 0; idx < n_axis; idx++) { - print_position[idx] -= wco[idx]; + position[idx] -= wco[idx]; } - - forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version } + float* get_wco() { static float wco[MAX_N_AXIS]; auto n_axis = number_axis->get(); @@ -988,5 +953,3 @@ float* get_wco() { } return wco; } - -void __attribute__((weak)) forward_kinematics(float* position) {} // This version does nothing. Make your own to do something with it diff --git a/Grbl_Esp32/src/Report.h b/Grbl_Esp32/src/Report.h index 366e8d9d..eecd905a 100644 --- a/Grbl_Esp32/src/Report.h +++ b/Grbl_Esp32/src/Report.h @@ -92,9 +92,6 @@ void report_grbl_settings(uint8_t client, uint8_t show_extended); // Prints an echo of the pre-parsed line received right before execution. void report_echo_line_received(char* line, uint8_t client); -// calculate the postion for status reports -void report_calc_status_position(float* print_position, float* wco, bool wpos); - // Prints realtime status report void report_realtime_status(uint8_t client); @@ -134,5 +131,4 @@ void reportTaskStackSize(UBaseType_t& saved); char* report_state_text(); float* get_wco(); -void calc_mpos(float* print_position); -void calc_wpos(float* print_position); +void mpos_to_wpos(float* position); diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index 60a399b2..74b593fd 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -30,6 +30,7 @@ volatile ExecState sys_rt_exec_state; // Global realtime executor bitflag v volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. volatile ExecAccessory sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. volatile bool cycle_stop; // For state transitions, instead of bitflag +volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while cartesian_to_motors has taken ownership of a line motion #ifdef DEBUG volatile bool sys_rt_exec_debug; #endif @@ -166,9 +167,6 @@ void system_flag_wco_change() { sys.report_wco_counter = 0; } -// Returns machine position of axis 'idx'. Must be sent a 'step' array. -// NOTE: If motor steps and machine position are not in the same coordinate frame, this function -// serves as a central place to compute the transformation. 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(); @@ -176,14 +174,22 @@ float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx) { return pos; } +// Returns machine position of axis 'idx'. Must be sent a 'step' array. +// NOTE: If motor steps and machine position are not in the same coordinate frame, this function +// serves as a central place to compute the transformation. void system_convert_array_steps_to_mpos(float* position, int32_t* steps) { - uint8_t idx; - auto n_axis = number_axis->get(); - for (idx = 0; idx < n_axis; idx++) { - position[idx] = system_convert_axis_steps_to_mpos(steps, idx); + auto n_axis = number_axis->get(); + float motors[n_axis]; + for (int idx = 0; idx < n_axis; idx++) { + motors[idx] = (float)steps[idx] / axis_settings[idx]->steps_per_mm->get(); } - return; + motors_to_cartesian(position, motors, n_axis); } +float* system_get_mpos() { + static float position[MAX_N_AXIS]; + system_convert_array_steps_to_mpos(position, sys_position); + return position; +}; // Returns control pin state as a uint8 bitfield. Each bit indicates the input pin state, where // triggered is 1 and not triggered is 0. Invert mask is applied. Bitfield organization is diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index d955f01e..c9f222ff 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -138,6 +138,7 @@ extern volatile Percent sys_rt_f_override; // Feed override extern volatile Percent sys_rt_r_override; // Rapid feed override value in percent extern volatile Percent sys_rt_s_override; // Spindle override value in percent extern volatile bool cycle_stop; +extern volatile void* sys_pl_data_inflight; // holds a plan_line_data_t while cartesian_to_motors has taken ownership of a line motion #ifdef DEBUG extern volatile bool sys_rt_exec_debug; #endif @@ -164,7 +165,8 @@ void system_flag_wco_change(); 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); +void system_convert_array_steps_to_mpos(float* position, int32_t* steps); +float* system_get_mpos(); // A task that runs after a control switch interrupt for debouncing. void controlCheckTask(void* pvParameters);