From 447d1739f3da9cd18004b7a92c9fc3441fba2147 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 5 Feb 2021 14:53:02 -0600 Subject: [PATCH] Core XY fixes (#756) * Updates for CoreXY * Delete fystec_ant.h --- Grbl_Esp32/Custom/CoreXY.cpp | 71 +++++++++++++++++++++++--------- Grbl_Esp32/src/Grbl.cpp | 7 ++-- Grbl_Esp32/src/Grbl.h | 7 ++-- Grbl_Esp32/src/Limits.cpp | 4 ++ Grbl_Esp32/src/MotionControl.cpp | 4 +- Grbl_Esp32/src/Report.cpp | 6 +-- 6 files changed, 67 insertions(+), 32 deletions(-) diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp index 238a46df..b1c6fb87 100644 --- a/Grbl_Esp32/Custom/CoreXY.cpp +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -69,7 +69,8 @@ bool user_defined_homing(uint8_t cycle_mask) { // check for multi axis homing per cycle ($Homing/Cycle0=XY type)...not allowed in CoreXY bool setting_error = false; - for (int cycle = 0; cycle < 3; cycle++) { + auto n_axis = number_axis->get(); + for (int cycle = 0; cycle < n_axis; cycle++) { if (numberOfSetBits(homing_cycle[cycle]->get()) > 1) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, @@ -90,7 +91,7 @@ bool user_defined_homing(uint8_t cycle_mask) { pl_data->motion.systemMotion = 1; pl_data->motion.noFeedOverride = 1; - uint8_t cycle_count = (cycle_mask == 0) ? 3 : 1; // if we have a cycle_mask, we are only going to do one axis + uint8_t cycle_count = (cycle_mask == 0) ? n_axis : 1; // if we have a cycle_mask, we are only going to do one axis AxisMask mask = 0; for (int cycle = 0; cycle < cycle_count; cycle++) { @@ -105,7 +106,7 @@ bool user_defined_homing(uint8_t cycle_mask) { mask = motors_set_homing_mode(mask, true); // non standard homing motors will do their own thing and get removed from the mask - for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) { + for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) { if (bit(axis) == mask) { // setup for the homing of this axis bool approach = true; @@ -129,7 +130,9 @@ bool user_defined_homing(uint8_t cycle_mask) { approach ? target[axis] = max_travel : target[axis] = -max_travel; } - target[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); + for (int axis = Z_AXIS; axis < n_axis; axis++) { + target[axis] = system_convert_axis_steps_to_mpos(sys_position, axis); + } // convert back to motor steps inverse_kinematics(target); @@ -217,7 +220,10 @@ bool user_defined_homing(uint8_t cycle_mask) { last_cartesian[X_AXIS] = target[X_AXIS]; last_cartesian[Y_AXIS] = target[Y_AXIS]; - last_cartesian[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); + + for (int axis = Z_AXIS; axis < n_axis; axis++) { + last_cartesian[axis] = system_convert_axis_steps_to_mpos(sys_position, axis); + } // convert to motors inverse_kinematics(target); @@ -239,15 +245,15 @@ bool user_defined_homing(uint8_t cycle_mask) { // This function is used by Grbl convert Cartesian to motors // this does not do any motion control void inverse_kinematics(float* position) { - float motors[3]; + float motors[MAX_N_AXIS]; motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; - motors[Z_AXIS] = position[Z_AXIS]; - position[0] = motors[0]; - position[1] = motors[1]; - position[2] = motors[2]; + position[X_AXIS] = motors[X_AXIS]; + position[Y_AXIS] = motors[Y_AXIS]; + + // Z and higher just pass through unchanged } // Inverse Kinematics calculates motor positions from real world cartesian positions @@ -268,7 +274,11 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS]; motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_AXIS]; - motors[Z_AXIS] = target[Z_AXIS]; + + auto n_axis = number_axis->get(); + for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) { + motors[axis] = target[axis]; + } float motor_distance = three_axis_dist(motors, last_motors); @@ -283,14 +293,36 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // motors -> cartesian void forward_kinematics(float* position) { - float calc_fwd[MAX_N_AXIS]; + float calc_fwd[MAX_N_AXIS]; + float wco[MAX_N_AXIS]; + float print_position[N_AXIS]; + int32_t current_position[N_AXIS]; // Copy current state of the system position variable + memcpy(current_position, sys_position, sizeof(sys_position)); + system_convert_array_steps_to_mpos(print_position, current_position); + + // determine the Work Coordinate Offsets for each axis + auto n_axis = number_axis->get(); + for (int axis = 0; axis < n_axis; axis++) { + // Apply work coordinate offsets and tool length offset to current position. + wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis]; + if (axis == TOOL_LENGTH_OFFSET_AXIS) { + wco[axis] += gc_state.tool_length_offset; + } + } + + // apply the forward kinemetics to the machine coordinates // https://corexy.com/theory.html - calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]); - calc_fwd[Y_AXIS] = 0.5 * (position[X_AXIS] - position[Y_AXIS]); + //calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]); + calc_fwd[X_AXIS] = ((0.5 * (print_position[X_AXIS] + print_position[Y_AXIS]) * geometry_factor) - wco[X_AXIS]); + calc_fwd[Y_AXIS] = ((0.5 * (print_position[X_AXIS] - print_position[Y_AXIS])) - wco[Y_AXIS]); - position[X_AXIS] = calc_fwd[X_AXIS]; - position[Y_AXIS] = calc_fwd[Y_AXIS]; + for (int axis = 0; axis < n_axis; axis++) { + if (axis > Y_AXIS) { // for axes above Y there is no kinematics + calc_fwd[axis] = print_position[axis] - wco[axis]; + } + position[axis] = calc_fwd[axis]; // this is the value returned to reporting + } } bool kinematics_pre_homing(uint8_t cycle_mask) { @@ -298,9 +330,10 @@ bool kinematics_pre_homing(uint8_t cycle_mask) { } void kinematics_post_homing() { - gc_state.position[X_AXIS] = last_cartesian[X_AXIS]; - gc_state.position[Y_AXIS] = last_cartesian[Y_AXIS]; - gc_state.position[Z_AXIS] = last_cartesian[Z_AXIS]; + auto n_axis = number_axis->get(); + for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) { + gc_state.position[axis] = last_cartesian[axis]; + } } // this is used used by Grbl soft limits to see if the range of the machine is exceeded. diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index 7a29bc4f..a338766f 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -42,10 +42,7 @@ void grbl_init() { init_motors(); system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files) memset(sys_position, 0, sizeof(sys_position)); // Clear machine position. - -#ifdef USE_MACHINE_INIT - machine_init(); // user supplied function for special initialization -#endif + machine_init(); // weak definition in Grbl.cpp does nothing // Initialize system state. #ifdef FORCE_INITIALIZATION_ALARM // Force Grbl into an ALARM state upon a power-cycle or hard reset. @@ -117,6 +114,8 @@ void run_once() { protocol_main_loop(); } +void __attribute__((weak)) machine_init() {} + /* setup() and loop() in the Arduino .ino implements this control flow: diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index f42b4b85..0d35d07c 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 = "20210203"; +const char* const GRBL_VERSION_BUILD = "20210204"; //#include #include @@ -94,8 +94,7 @@ void run_once(); // Called if USE_MACHINE_INIT is defined void machine_init(); -// Called if USE_CUSTOM_HOMING is defined -bool user_defined_homing(uint8_t cycle_mask); +bool user_defined_homing(uint8_t cycle_mask); // weak definition in Limits.cpp // Called if USE_KINEMATICS is defined @@ -106,7 +105,7 @@ uint8_t kinematic_limits_check(float* target); // Called if USE_FWD_KINEMATICS is defined void inverse_kinematics(float* position); // used to return a converted value -void forward_kinematics(float* position); +void forward_kinematics(float* position); // weak definition in Report.cpp // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined void user_defined_macro(uint8_t index); diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 029e3477..41596e9d 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -404,3 +404,7 @@ bool limitsCheckTravel(float* target) { } return false; } + +bool __attribute__((weak)) user_defined_homing(uint8_t cycle_mask) { + return false; +} diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 954009a1..3387ef23 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -283,11 +283,11 @@ static bool axis_is_squared(uint8_t axis_mask) { // executing the homing cycle. This prevents incorrect buffered plans after homing. void mc_homing_cycle(uint8_t cycle_mask) { bool no_cycles_defined = true; -#ifdef USE_CUSTOM_HOMING + if (user_defined_homing(cycle_mask)) { return; } -#endif + // This give kinematics a chance to do something before normal homing // if it returns true, the homing is canceled. #ifdef USE_KINEMATICS diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index da8b5316..892230e8 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -659,13 +659,11 @@ void report_realtime_status(uint8_t client) { } } } + forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version // Report machine position if (bit_istrue(status_mask->get(), RtStatus::Position)) { strcat(status, "|MPos:"); } else { -#ifdef USE_FWD_KINEMATICS - forward_kinematics(print_position); -#endif strcat(status, "|WPos:"); } report_util_axis_values(print_position, temp); @@ -957,3 +955,5 @@ void reportTaskStackSize(UBaseType_t& saved) { } #endif } + +void __attribute__((weak)) forward_kinematics(float* position) {} // This version does nothing. Make your own to do something with it