From c5cac486f5bd19840c301964d5d5ff5edc9a6613 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 9 Oct 2016 13:25:25 -0500 Subject: [PATCH] Use a, b, c instead of lx, ly, lz --- Marlin/Marlin_main.cpp | 3 +- Marlin/planner.cpp | 159 +++++++++++++++++++++-------------------- Marlin/planner.h | 29 ++++---- Marlin/stepper.cpp | 30 ++++---- Marlin/stepper.h | 6 +- 5 files changed, 116 insertions(+), 111 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 5a073145a..9be4fd9b1 100755 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -6892,8 +6892,7 @@ inline void gcode_M503() { KEEPALIVE_STATE(IN_HANDLER); // Set extruder to saved position - current_position[E_AXIS] = lastpos[E_AXIS]; - destination[E_AXIS] = lastpos[E_AXIS]; + destination[E_AXIS] = current_position[E_AXIS] = lastpos[E_AXIS]; planner.set_e_position_mm(current_position[E_AXIS]); #if IS_KINEMATIC diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 183fe0c17..d1cf89ea5 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -594,13 +594,14 @@ void Planner::check_axes_activity() { * Planner::_buffer_line * * Add a new linear movement to the buffer. - * Not apply the leveling. * - * x,y,z,e - target position in mm - * fr_mm_s - (target) speed of the move - * extruder - target extruder + * Leveling and kinematics should be applied ahead of calling this. + * + * a,b,c,e - target positions in mm or degrees + * fr_mm_s - (target) speed of the move + * extruder - target extruder */ -void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, const float &e, float fr_mm_s, const uint8_t extruder) { +void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder) { // Calculate the buffer head after we push this byte int next_buffer_head = next_block_index(block_buffer_head); @@ -611,36 +612,36 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co // The target position of the tool in absolute steps // Calculate target position in absolute steps //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow - long target[NUM_AXIS] = { - lround(lx * axis_steps_per_mm[X_AXIS]), - lround(ly * axis_steps_per_mm[Y_AXIS]), - lround(lz * axis_steps_per_mm[Z_AXIS]), + long target[XYZE] = { + lround(a * axis_steps_per_mm[X_AXIS]), + lround(b * axis_steps_per_mm[Y_AXIS]), + lround(c * axis_steps_per_mm[Z_AXIS]), lround(e * axis_steps_per_mm[E_AXIS]) }; - long dx = target[X_AXIS] - position[X_AXIS], - dy = target[Y_AXIS] - position[Y_AXIS], - dz = target[Z_AXIS] - position[Z_AXIS]; + long da = target[X_AXIS] - position[X_AXIS], + db = target[Y_AXIS] - position[Y_AXIS], + dc = target[Z_AXIS] - position[Z_AXIS]; /* SERIAL_ECHOPAIR(" Planner FR:", fr_mm_s); SERIAL_CHAR(' '); #if IS_KINEMATIC - SERIAL_ECHOPAIR("A:", lx); - SERIAL_ECHOPAIR(" (", dx); - SERIAL_ECHOPAIR(") B:", ly); + SERIAL_ECHOPAIR("A:", a); + SERIAL_ECHOPAIR(" (", da); + SERIAL_ECHOPAIR(") B:", b); #else - SERIAL_ECHOPAIR("X:", lx); - SERIAL_ECHOPAIR(" (", dx); - SERIAL_ECHOPAIR(") Y:", ly); + SERIAL_ECHOPAIR("X:", a); + SERIAL_ECHOPAIR(" (", da); + SERIAL_ECHOPAIR(") Y:", b); #endif - SERIAL_ECHOPAIR(" (", dy); + SERIAL_ECHOPAIR(" (", db); #if ENABLED(DELTA) - SERIAL_ECHOPAIR(") C:", lz); + SERIAL_ECHOPAIR(") C:", c); #else - SERIAL_ECHOPAIR(") Z:", lz); + SERIAL_ECHOPAIR(") Z:", c); #endif - SERIAL_ECHOPAIR(" (", dz); + SERIAL_ECHOPAIR(" (", dc); SERIAL_CHAR(')'); SERIAL_EOL; //*/ @@ -679,24 +680,24 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co #if ENABLED(COREXY) // corexy planning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html - block->steps[A_AXIS] = labs(dx + dy); - block->steps[B_AXIS] = labs(dx - dy); - block->steps[Z_AXIS] = labs(dz); + block->steps[A_AXIS] = labs(da + db); + block->steps[B_AXIS] = labs(da - db); + block->steps[Z_AXIS] = labs(dc); #elif ENABLED(COREXZ) // corexz planning - block->steps[A_AXIS] = labs(dx + dz); - block->steps[Y_AXIS] = labs(dy); - block->steps[C_AXIS] = labs(dx - dz); + block->steps[A_AXIS] = labs(da + dc); + block->steps[Y_AXIS] = labs(db); + block->steps[C_AXIS] = labs(da - dc); #elif ENABLED(COREYZ) // coreyz planning - block->steps[X_AXIS] = labs(dx); - block->steps[B_AXIS] = labs(dy + dz); - block->steps[C_AXIS] = labs(dy - dz); + block->steps[X_AXIS] = labs(da); + block->steps[B_AXIS] = labs(db + dc); + block->steps[C_AXIS] = labs(db - dc); #else // default non-h-bot planning - block->steps[X_AXIS] = labs(dx); - block->steps[Y_AXIS] = labs(dy); - block->steps[Z_AXIS] = labs(dz); + block->steps[X_AXIS] = labs(da); + block->steps[Y_AXIS] = labs(db); + block->steps[Z_AXIS] = labs(dc); #endif block->steps[E_AXIS] = labs(de) * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01 + 0.5; @@ -720,33 +721,33 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co block->e_to_p_pressure = baricuda_e_to_p_pressure; #endif - // Compute direction bits for this block - uint8_t db = 0; + // Compute direction bit-mask for this block + uint8_t dm = 0; #if ENABLED(COREXY) - if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis - if (dy < 0) SBI(db, Y_HEAD); // ...and Y - if (dz < 0) SBI(db, Z_AXIS); - if (dx + dy < 0) SBI(db, A_AXIS); // Motor A direction - if (dx - dy < 0) SBI(db, B_AXIS); // Motor B direction + if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis + if (db < 0) SBI(dm, Y_HEAD); // ...and Y + if (dc < 0) SBI(dm, Z_AXIS); + if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction + if (da - db < 0) SBI(dm, B_AXIS); // Motor B direction #elif ENABLED(COREXZ) - if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis - if (dy < 0) SBI(db, Y_AXIS); - if (dz < 0) SBI(db, Z_HEAD); // ...and Z - if (dx + dz < 0) SBI(db, A_AXIS); // Motor A direction - if (dx - dz < 0) SBI(db, C_AXIS); // Motor C direction + if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis + if (db < 0) SBI(dm, Y_AXIS); + if (dc < 0) SBI(dm, Z_HEAD); // ...and Z + if (da + dc < 0) SBI(dm, A_AXIS); // Motor A direction + if (da - dc < 0) SBI(dm, C_AXIS); // Motor C direction #elif ENABLED(COREYZ) - if (dx < 0) SBI(db, X_AXIS); - if (dy < 0) SBI(db, Y_HEAD); // Save the real Extruder (head) direction in Y Axis - if (dz < 0) SBI(db, Z_HEAD); // ...and Z - if (dy + dz < 0) SBI(db, B_AXIS); // Motor B direction - if (dy - dz < 0) SBI(db, C_AXIS); // Motor C direction + if (da < 0) SBI(dm, X_AXIS); + if (db < 0) SBI(dm, Y_HEAD); // Save the real Extruder (head) direction in Y Axis + if (dc < 0) SBI(dm, Z_HEAD); // ...and Z + if (db + dc < 0) SBI(dm, B_AXIS); // Motor B direction + if (db - dc < 0) SBI(dm, C_AXIS); // Motor C direction #else - if (dx < 0) SBI(db, X_AXIS); - if (dy < 0) SBI(db, Y_AXIS); - if (dz < 0) SBI(db, Z_AXIS); + if (da < 0) SBI(dm, X_AXIS); + if (db < 0) SBI(dm, Y_AXIS); + if (dc < 0) SBI(dm, Z_AXIS); #endif - if (de < 0) SBI(db, E_AXIS); - block->direction_bits = db; + if (de < 0) SBI(dm, E_AXIS); + block->direction_bits = dm; block->active_extruder = extruder; @@ -859,29 +860,29 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ) float delta_mm[7]; #if ENABLED(COREXY) - delta_mm[X_HEAD] = dx * steps_to_mm[A_AXIS]; - delta_mm[Y_HEAD] = dy * steps_to_mm[B_AXIS]; - delta_mm[Z_AXIS] = dz * steps_to_mm[Z_AXIS]; - delta_mm[A_AXIS] = (dx + dy) * steps_to_mm[A_AXIS]; - delta_mm[B_AXIS] = (dx - dy) * steps_to_mm[B_AXIS]; + delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS]; + delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS]; + delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS]; + delta_mm[A_AXIS] = (da + db) * steps_to_mm[A_AXIS]; + delta_mm[B_AXIS] = (da - db) * steps_to_mm[B_AXIS]; #elif ENABLED(COREXZ) - delta_mm[X_HEAD] = dx * steps_to_mm[A_AXIS]; - delta_mm[Y_AXIS] = dy * steps_to_mm[Y_AXIS]; - delta_mm[Z_HEAD] = dz * steps_to_mm[C_AXIS]; - delta_mm[A_AXIS] = (dx + dz) * steps_to_mm[A_AXIS]; - delta_mm[C_AXIS] = (dx - dz) * steps_to_mm[C_AXIS]; + delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS]; + delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS]; + delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS]; + delta_mm[A_AXIS] = (da + dc) * steps_to_mm[A_AXIS]; + delta_mm[C_AXIS] = (da - dc) * steps_to_mm[C_AXIS]; #elif ENABLED(COREYZ) - delta_mm[X_AXIS] = dx * steps_to_mm[X_AXIS]; - delta_mm[Y_HEAD] = dy * steps_to_mm[B_AXIS]; - delta_mm[Z_HEAD] = dz * steps_to_mm[C_AXIS]; - delta_mm[B_AXIS] = (dy + dz) * steps_to_mm[B_AXIS]; - delta_mm[C_AXIS] = (dy - dz) * steps_to_mm[C_AXIS]; + delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS]; + delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS]; + delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS]; + delta_mm[B_AXIS] = (db + dc) * steps_to_mm[B_AXIS]; + delta_mm[C_AXIS] = (db - dc) * steps_to_mm[C_AXIS]; #endif #else float delta_mm[4]; - delta_mm[X_AXIS] = dx * steps_to_mm[X_AXIS]; - delta_mm[Y_AXIS] = dy * steps_to_mm[Y_AXIS]; - delta_mm[Z_AXIS] = dz * steps_to_mm[Z_AXIS]; + delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS]; + delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS]; + delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS]; #endif delta_mm[E_AXIS] = 0.01 * (de * steps_to_mm[E_AXIS]) * volumetric_multiplier[extruder] * flow_percentage[extruder]; @@ -1184,12 +1185,12 @@ void Planner::_buffer_line(const float &lx, const float &ly, const float &lz, co * On CORE machines stepper ABC will be translated from the given XYZ. */ -void Planner::_set_position_mm(const float &lx, const float &ly, const float &lz, const float &e) { - long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]), - ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]), - nz = position[Z_AXIS] = lround(lz * axis_steps_per_mm[Z_AXIS]), +void Planner::_set_position_mm(const float &a, const float &b, const float &c, const float &e) { + long na = position[X_AXIS] = lround(a * axis_steps_per_mm[X_AXIS]), + nb = position[Y_AXIS] = lround(b * axis_steps_per_mm[Y_AXIS]), + nc = position[Z_AXIS] = lround(c * axis_steps_per_mm[Z_AXIS]), ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]); - stepper.set_position(nx, ny, nz, ne); + stepper.set_position(na, nb, nc, ne); previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. memset(previous_speed, 0, sizeof(previous_speed)); diff --git a/Marlin/planner.h b/Marlin/planner.h index 1d7e65a25..785c6ac51 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -232,27 +232,32 @@ class Planner { /** * Planner::_buffer_line * - * Add a new linear movement to the buffer. - * Doesn't apply the leveling. + * Add a new direct linear movement to the buffer. + * + * Leveling and kinematics should be applied ahead of this. * - * x,y,z,e - target position in mm - * fr_mm_s - (target) speed of the move + * a,b,c,e - target position in mm or degrees + * fr_mm_s - (target) speed of the move (mm/s) * extruder - target extruder */ - static void _buffer_line(const float &lx, const float &ly, const float &lz, const float &e, float fr_mm_s, const uint8_t extruder); + static void _buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder); - static void _set_position_mm(const float &lx, const float &ly, const float &lz, const float &e); + static void _set_position_mm(const float &a, const float &b, const float &c, const float &e); /** * Add a new linear movement to the buffer. * The target is NOT translated to delta/scara * - * x,y,z,e - target position in mm - * fr_mm_s - (target) speed of the move (mm/s) - * extruder - target extruder + * Leveling will be applied to input on cartesians. + * Kinematic machines should call buffer_line_kinematic (for leveled moves). + * (Cartesians may also call buffer_line_kinematic.) + * + * lx,ly,lz,e - target position in mm or degrees + * fr_mm_s - (target) speed of the move (mm/s) + * extruder - target extruder */ static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) { - #if PLANNER_LEVELING && ! IS_KINEMATIC + #if PLANNER_LEVELING && IS_CARTESIAN apply_leveling(lx, ly, lz); #endif _buffer_line(lx, ly, lz, e, fr_mm_s, extruder); @@ -267,7 +272,7 @@ class Planner { * fr_mm_s - (target) speed of the move (mm/s) * extruder - target extruder */ - static FORCE_INLINE void buffer_line_kinematic(const float target[NUM_AXIS], float fr_mm_s, const uint8_t extruder) { + static FORCE_INLINE void buffer_line_kinematic(const float target[XYZE], float fr_mm_s, const uint8_t extruder) { #if PLANNER_LEVELING float pos[XYZ] = { target[X_AXIS], target[Y_AXIS], target[Z_AXIS] }; apply_leveling(pos); @@ -292,7 +297,7 @@ class Planner { * Clears previous speed values. */ static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) { - #if PLANNER_LEVELING && ! IS_KINEMATIC + #if PLANNER_LEVELING && IS_CARTESIAN apply_leveling(lx, ly, lz); #endif _set_position_mm(lx, ly, lz, e); diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index f2800ebe5..bd4aa85b6 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -953,7 +953,7 @@ void Stepper::synchronize() { while (planner.blocks_queued()) idle(); } * This allows get_axis_position_mm to correctly * derive the current XYZ position later on. */ -void Stepper::set_position(const long& x, const long& y, const long& z, const long& e) { +void Stepper::set_position(const long &a, const long &b, const long &c, const long &e) { synchronize(); // Bad to set stepper counts in the middle of a move @@ -962,37 +962,37 @@ void Stepper::set_position(const long& x, const long& y, const long& z, const lo #if ENABLED(COREXY) // corexy positioning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html - count_position[A_AXIS] = x + y; - count_position[B_AXIS] = x - y; - count_position[Z_AXIS] = z; + count_position[A_AXIS] = a + b; + count_position[B_AXIS] = a - b; + count_position[Z_AXIS] = c; #elif ENABLED(COREXZ) // corexz planning - count_position[A_AXIS] = x + z; - count_position[Y_AXIS] = y; - count_position[C_AXIS] = x - z; + count_position[A_AXIS] = a + c; + count_position[Y_AXIS] = b; + count_position[C_AXIS] = a - c; #elif ENABLED(COREYZ) // coreyz planning - count_position[X_AXIS] = x; - count_position[B_AXIS] = y + z; - count_position[C_AXIS] = y - z; + count_position[X_AXIS] = a; + count_position[B_AXIS] = y + c; + count_position[C_AXIS] = y - c; #else // default non-h-bot planning - count_position[X_AXIS] = x; - count_position[Y_AXIS] = y; - count_position[Z_AXIS] = z; + count_position[X_AXIS] = a; + count_position[Y_AXIS] = b; + count_position[Z_AXIS] = c; #endif count_position[E_AXIS] = e; CRITICAL_SECTION_END; } -void Stepper::set_position(const AxisEnum &axis, const long& v) { +void Stepper::set_position(const AxisEnum &axis, const long &v) { CRITICAL_SECTION_START; count_position[axis] = v; CRITICAL_SECTION_END; } -void Stepper::set_e_position(const long& e) { +void Stepper::set_e_position(const long &e) { CRITICAL_SECTION_START; count_position[E_AXIS] = e; CRITICAL_SECTION_END; diff --git a/Marlin/stepper.h b/Marlin/stepper.h index 3e31f82d1..77e67fcad 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -188,9 +188,9 @@ class Stepper { // // Set the current position in steps // - static void set_position(const long& x, const long& y, const long& z, const long& e); - static void set_position(const AxisEnum& a, const long& v); - static void set_e_position(const long& e); + static void set_position(const long &a, const long &b, const long &c, const long &e); + static void set_position(const AxisEnum &a, const long &v); + static void set_e_position(const long &e); // // Set direction bits for all steppers