From 8f8c6a9bc4f1c8b2d927e74339a48217d9b48ec7 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 12 May 2018 09:59:11 -0500 Subject: [PATCH] Move get_axis_position_mm to Planner (#10718) --- Marlin/src/core/utility.cpp | 6 ++-- Marlin/src/feature/I2CPositionEncoder.cpp | 24 +++++++------- .../src/feature/bedlevel/ubl/ubl_motion.cpp | 4 +-- Marlin/src/gcode/host/M114.cpp | 6 ++-- Marlin/src/module/motion.cpp | 23 ++++++-------- Marlin/src/module/planner.cpp | 31 +++++++++++++++++++ Marlin/src/module/planner.h | 11 +++++++ Marlin/src/module/scara.cpp | 6 ++-- Marlin/src/module/stepper.cpp | 26 ---------------- Marlin/src/module/stepper.h | 12 ------- Marlin/src/module/temperature.cpp | 2 +- Marlin/src/module/temperature.h | 4 --- 12 files changed, 75 insertions(+), 80 deletions(-) diff --git a/Marlin/src/core/utility.cpp b/Marlin/src/core/utility.cpp index 16bc78d92f..89d22caeb2 100644 --- a/Marlin/src/core/utility.cpp +++ b/Marlin/src/core/utility.cpp @@ -338,9 +338,9 @@ void safe_delay(millis_t ms) { #endif #if ABL_PLANAR const float diff[XYZ] = { - stepper.get_axis_position_mm(X_AXIS) - current_position[X_AXIS], - stepper.get_axis_position_mm(Y_AXIS) - current_position[Y_AXIS], - stepper.get_axis_position_mm(Z_AXIS) - current_position[Z_AXIS] + planner.get_axis_position_mm(X_AXIS) - current_position[X_AXIS], + planner.get_axis_position_mm(Y_AXIS) - current_position[Y_AXIS], + planner.get_axis_position_mm(Z_AXIS) - current_position[Z_AXIS] }; SERIAL_ECHOPGM("ABL Adjustment X"); if (diff[X_AXIS] > 0) SERIAL_CHAR('+'); diff --git a/Marlin/src/feature/I2CPositionEncoder.cpp b/Marlin/src/feature/I2CPositionEncoder.cpp index 352fb369b3..b1778ae582 100644 --- a/Marlin/src/feature/I2CPositionEncoder.cpp +++ b/Marlin/src/feature/I2CPositionEncoder.cpp @@ -99,15 +99,15 @@ void I2CPositionEncoder::update() { //the encoder likely lost its place when the error occured, so we'll reset and use the printer's //idea of where it the axis is to re-initialise - float position = stepper.get_axis_position_mm(encoderAxis); - int32_t positionInTicks = position * get_ticks_unit(); + const float pos = planner.get_axis_position_mm(encoderAxis); + int32_t positionInTicks = pos * get_ticks_unit(); //shift position from previous to current position zeroOffset -= (positionInTicks - get_position()); #ifdef I2CPE_DEBUG SERIAL_ECHOPGM("Current position is "); - SERIAL_ECHOLN(position); + SERIAL_ECHOLN(pos); SERIAL_ECHOPGM("Position in encoder ticks is "); SERIAL_ECHOLN(positionInTicks); @@ -254,7 +254,7 @@ bool I2CPositionEncoder::passes_test(const bool report) { float I2CPositionEncoder::get_axis_error_mm(const bool report) { float target, actual, error; - target = stepper.get_axis_position_mm(encoderAxis); + target = planner.get_axis_position_mm(encoderAxis); actual = mm_from_count(position); error = actual - target; @@ -349,8 +349,8 @@ bool I2CPositionEncoder::test_axis() { ec = false; LOOP_NA(i) { - startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); - endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); + startCoord[i] = planner.get_axis_position_mm((AxisEnum)i); + endCoord[i] = planner.get_axis_position_mm((AxisEnum)i); } startCoord[encoderAxis] = startPosition; @@ -359,7 +359,7 @@ bool I2CPositionEncoder::test_axis() { planner.synchronize(); planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS], - stepper.get_axis_position_mm(E_AXIS), feedrate, 0); + planner.get_axis_position_mm(E_AXIS), feedrate, 0); planner.synchronize(); // if the module isn't currently trusted, wait until it is (or until it should be if things are working) @@ -371,7 +371,7 @@ bool I2CPositionEncoder::test_axis() { if (trusted) { // if trusted, commence test planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS], - stepper.get_axis_position_mm(E_AXIS), feedrate, 0); + planner.get_axis_position_mm(E_AXIS), feedrate, 0); planner.synchronize(); } @@ -408,8 +408,8 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { travelDistance = endDistance - startDistance; LOOP_NA(i) { - startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); - endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); + startCoord[i] = planner.get_axis_position_mm((AxisEnum)i); + endCoord[i] = planner.get_axis_position_mm((AxisEnum)i); } startCoord[encoderAxis] = startDistance; @@ -419,7 +419,7 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { LOOP_L_N(i, iter) { planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS], - stepper.get_axis_position_mm(E_AXIS), feedrate, 0); + planner.get_axis_position_mm(E_AXIS), feedrate, 0); planner.synchronize(); delay(250); @@ -428,7 +428,7 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { //do_blocking_move_to(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS]); planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS], - stepper.get_axis_position_mm(E_AXIS), feedrate, 0); + planner.get_axis_position_mm(E_AXIS), feedrate, 0); planner.synchronize(); //Read encoder distance diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp index 44fd02a4c5..957a3a7d1d 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp @@ -448,8 +448,8 @@ #if IS_SCARA // scale the feed rate from mm/s to degrees/s scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate; - scara_oldA = stepper.get_axis_position_degrees(A_AXIS); - scara_oldB = stepper.get_axis_position_degrees(B_AXIS); + scara_oldA = planner.get_axis_position_degrees(A_AXIS); + scara_oldB = planner.get_axis_position_degrees(B_AXIS); #endif const float diff[XYZE] = { diff --git a/Marlin/src/gcode/host/M114.cpp b/Marlin/src/gcode/host/M114.cpp index 154b280550..e273c4e101 100644 --- a/Marlin/src/gcode/host/M114.cpp +++ b/Marlin/src/gcode/host/M114.cpp @@ -90,8 +90,8 @@ #if IS_SCARA const float deg[XYZ] = { - stepper.get_axis_position_degrees(A_AXIS), - stepper.get_axis_position_degrees(B_AXIS) + planner.get_axis_position_degrees(A_AXIS), + planner.get_axis_position_degrees(B_AXIS) }; SERIAL_PROTOCOLPGM("Degrees:"); report_xyze(deg, 2); @@ -99,7 +99,7 @@ SERIAL_PROTOCOLPGM("FromStp:"); get_cartesian_from_steppers(); // writes cartes[XYZ] (with forward kinematics) - const float from_steppers[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], stepper.get_axis_position_mm(E_AXIS) }; + const float from_steppers[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], planner.get_axis_position_mm(E_AXIS) }; report_xyze(from_steppers); const float diff[XYZE] = { diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index cbaaf82bfe..e9818e776b 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -193,21 +193,21 @@ void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS] void get_cartesian_from_steppers() { #if ENABLED(DELTA) forward_kinematics_DELTA( - stepper.get_axis_position_mm(A_AXIS), - stepper.get_axis_position_mm(B_AXIS), - stepper.get_axis_position_mm(C_AXIS) + planner.get_axis_position_mm(A_AXIS), + planner.get_axis_position_mm(B_AXIS), + planner.get_axis_position_mm(C_AXIS) ); #else #if IS_SCARA forward_kinematics_SCARA( - stepper.get_axis_position_degrees(A_AXIS), - stepper.get_axis_position_degrees(B_AXIS) + planner.get_axis_position_degrees(A_AXIS), + planner.get_axis_position_degrees(B_AXIS) ); #else - cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); - cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS); + cartes[X_AXIS] = planner.get_axis_position_mm(X_AXIS); + cartes[Y_AXIS] = planner.get_axis_position_mm(Y_AXIS); #endif - cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS); + cartes[Z_AXIS] = planner.get_axis_position_mm(Z_AXIS); #endif } @@ -870,12 +870,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, } #endif // move duplicate extruder into correct duplication position. - planner.set_position_mm( - inactive_extruder_x_pos, - current_position[Y_AXIS], - current_position[Z_AXIS], - current_position[E_AXIS] - ); + planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); planner.buffer_line( current_position[X_AXIS] + duplicate_extruder_x_offset, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 99d814ce4b..ac8ae8aaaa 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1299,6 +1299,37 @@ void Planner::check_axes_activity() { #endif // PLANNER_LEVELING +/** + * Get an axis position according to stepper position(s) + * For CORE machines apply translation from ABC to XYZ. + */ +float Planner::get_axis_position_mm(const AxisEnum axis) { + float axis_steps; + #if IS_CORE + // Requesting one of the "core" axes? + if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) { + + // Protect the access to the position. + const bool was_enabled = STEPPER_ISR_ENABLED(); + DISABLE_STEPPER_DRIVER_INTERRUPT(); + + // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1 + // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2 + axis_steps = 0.5f * ( + axis == CORE_AXIS_2 ? CORESIGN(stepper.position(CORE_AXIS_1) - stepper.position(CORE_AXIS_2)) + : stepper.position(CORE_AXIS_1) + stepper.position(CORE_AXIS_2) + ); + + if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); + } + else + axis_steps = stepper.position(axis); + #else + axis_steps = stepper.position(axis); + #endif + return axis_steps * steps_to_mm[axis]; +} + /** * Block until all buffered steps are executed / cleaned */ diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 148ca93bdb..e7fe34031b 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -546,6 +546,17 @@ class Planner { */ static void sync_from_steppers(); + /** + * Get an axis position according to stepper position(s) + * For CORE machines apply translation from ABC to XYZ. + */ + static float get_axis_position_mm(const AxisEnum axis); + + // SCARA AB axes are in degrees, not mm + #if IS_SCARA + FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); } + #endif + /** * Does the buffer have any blocks queued? */ diff --git a/Marlin/src/module/scara.cpp b/Marlin/src/module/scara.cpp index 8206388a40..222bf9cbe4 100644 --- a/Marlin/src/module/scara.cpp +++ b/Marlin/src/module/scara.cpp @@ -30,7 +30,7 @@ #include "scara.h" #include "motion.h" -#include "stepper.h" +#include "planner.h" float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND; @@ -147,8 +147,8 @@ void inverse_kinematics(const float raw[XYZ]) { } void scara_report_positions() { - SERIAL_PROTOCOLPAIR("SCARA Theta:", stepper.get_axis_position_degrees(A_AXIS)); - SERIAL_PROTOCOLLNPAIR(" Psi+Theta:", stepper.get_axis_position_degrees(B_AXIS)); + SERIAL_PROTOCOLPAIR("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS)); + SERIAL_PROTOCOLLNPAIR(" Psi+Theta:", planner.get_axis_position_degrees(B_AXIS)); SERIAL_EOL(); } diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index d62081d854..7af6b2c94d 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -2037,32 +2037,6 @@ int32_t Stepper::position(const AxisEnum axis) { return count_pos; } -/** - * Get an axis position according to stepper position(s) - * For CORE machines apply translation from ABC to XYZ. - */ -float Stepper::get_axis_position_mm(const AxisEnum axis) { - float axis_steps; - #if IS_CORE - // Requesting one of the "core" axes? - if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) { - CRITICAL_SECTION_START; - // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1 - // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2 - axis_steps = 0.5f * ( - axis == CORE_AXIS_2 ? CORESIGN(count_position[CORE_AXIS_1] - count_position[CORE_AXIS_2]) - : count_position[CORE_AXIS_1] + count_position[CORE_AXIS_2] - ); - CRITICAL_SECTION_END; - } - else - axis_steps = position(axis); - #else - axis_steps = position(axis); - #endif - return axis_steps * planner.steps_to_mm[axis]; -} - void Stepper::finish_and_disable() { planner.synchronize(); disable_all_steppers(); diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 0df42d24b8..795a93dbe2 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -228,18 +228,6 @@ class Stepper { // static void report_positions(); - // - // Get the position (mm) of an axis based on stepper position(s) - // - static float get_axis_position_mm(const AxisEnum axis); - - // - // SCARA AB axes are in degrees, not mm - // - #if IS_SCARA - FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); } - #endif - // // The stepper subsystem goes to sleep when it runs out of things to execute. Call this // to notify the subsystem that it is time to go to work. diff --git a/Marlin/src/module/temperature.cpp b/Marlin/src/module/temperature.cpp index 46f3dc58c3..a55c803ebb 100644 --- a/Marlin/src/module/temperature.cpp +++ b/Marlin/src/module/temperature.cpp @@ -36,7 +36,7 @@ #include "../libs/private_spi.h" #endif -#if ENABLED(BABYSTEPPING) +#if ENABLED(BABYSTEPPING) || ENABLED(PID_EXTRUSION_SCALING) #include "stepper.h" #endif diff --git a/Marlin/src/module/temperature.h b/Marlin/src/module/temperature.h index fd461df58b..b3e4e3cffd 100644 --- a/Marlin/src/module/temperature.h +++ b/Marlin/src/module/temperature.h @@ -38,10 +38,6 @@ #include "../feature/power.h" #endif -#if ENABLED(PID_EXTRUSION_SCALING) - #include "stepper.h" -#endif - #ifndef SOFT_PWM_SCALE #define SOFT_PWM_SCALE 0 #endif