|
|
|
@ -712,24 +712,24 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const |
|
|
|
|
|
|
|
// Compute direction bit-mask for this block
|
|
|
|
uint8_t dm = 0; |
|
|
|
#if ENABLED(COREXY) |
|
|
|
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 CORE_IS_XY |
|
|
|
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 (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
|
|
|
|
if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction
|
|
|
|
if (CORESIGN(da - db) < 0) SBI(dm, B_AXIS); // Motor B direction
|
|
|
|
#elif CORE_IS_XZ |
|
|
|
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 (dc < 0) SBI(dm, Z_HEAD); // ...and Z
|
|
|
|
if (da + dc < 0) SBI(dm, A_AXIS); // Motor A direction
|
|
|
|
if (CORESIGN(da - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
|
|
|
|
#elif CORE_IS_YZ |
|
|
|
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
|
|
|
|
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 (CORESIGN(db - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
|
|
|
|
#else |
|
|
|
if (da < 0) SBI(dm, X_AXIS); |
|
|
|
if (db < 0) SBI(dm, Y_AXIS); |
|
|
|
@ -757,19 +757,16 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const |
|
|
|
block->direction_bits = dm; |
|
|
|
|
|
|
|
// Number of steps for each axis
|
|
|
|
#if ENABLED(COREXY) |
|
|
|
// corexy planning
|
|
|
|
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
|
|
|
|
// See http://www.corexy.com/theory.html
|
|
|
|
#if CORE_IS_XY |
|
|
|
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
|
|
|
|
#elif CORE_IS_XZ |
|
|
|
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
|
|
|
|
#elif CORE_IS_YZ |
|
|
|
block->steps[X_AXIS] = labs(da); |
|
|
|
block->steps[B_AXIS] = labs(db + dc); |
|
|
|
block->steps[C_AXIS] = labs(db - dc); |
|
|
|
@ -804,7 +801,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const |
|
|
|
block->active_extruder = extruder; |
|
|
|
|
|
|
|
//enable active axes
|
|
|
|
#if ENABLED(COREXY) |
|
|
|
#if CORE_IS_XY |
|
|
|
if (block->steps[A_AXIS] || block->steps[B_AXIS]) { |
|
|
|
enable_x(); |
|
|
|
enable_y(); |
|
|
|
@ -812,13 +809,13 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const |
|
|
|
#if DISABLED(Z_LATE_ENABLE) |
|
|
|
if (block->steps[Z_AXIS]) enable_z(); |
|
|
|
#endif |
|
|
|
#elif ENABLED(COREXZ) |
|
|
|
#elif CORE_IS_XZ |
|
|
|
if (block->steps[A_AXIS] || block->steps[C_AXIS]) { |
|
|
|
enable_x(); |
|
|
|
enable_z(); |
|
|
|
} |
|
|
|
if (block->steps[Y_AXIS]) enable_y(); |
|
|
|
#elif ENABLED(COREYZ) |
|
|
|
#elif CORE_IS_YZ |
|
|
|
if (block->steps[B_AXIS] || block->steps[C_AXIS]) { |
|
|
|
enable_y(); |
|
|
|
enable_z(); |
|
|
|
@ -915,26 +912,26 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const |
|
|
|
* So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head. |
|
|
|
* Having the real displacement of the head, we can calculate the total movement length and apply the desired speed. |
|
|
|
*/ |
|
|
|
#if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ) |
|
|
|
#if IS_CORE |
|
|
|
float delta_mm[7]; |
|
|
|
#if ENABLED(COREXY) |
|
|
|
#if CORE_IS_XY |
|
|
|
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[B_AXIS] = CORESIGN(da - db) * steps_to_mm[B_AXIS]; |
|
|
|
#elif CORE_IS_XZ |
|
|
|
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[C_AXIS] = CORESIGN(da - dc) * steps_to_mm[C_AXIS]; |
|
|
|
#elif CORE_IS_YZ |
|
|
|
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]; |
|
|
|
delta_mm[C_AXIS] = CORESIGN(db - dc) * steps_to_mm[C_AXIS]; |
|
|
|
#endif |
|
|
|
#else |
|
|
|
float delta_mm[4]; |
|
|
|
@ -949,11 +946,11 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const |
|
|
|
} |
|
|
|
else { |
|
|
|
block->millimeters = sqrt( |
|
|
|
#if ENABLED(COREXY) |
|
|
|
#if CORE_IS_XY |
|
|
|
sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_AXIS]) |
|
|
|
#elif ENABLED(COREXZ) |
|
|
|
#elif CORE_IS_XZ |
|
|
|
sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_HEAD]) |
|
|
|
#elif ENABLED(COREYZ) |
|
|
|
#elif CORE_IS_YZ |
|
|
|
sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_HEAD]) |
|
|
|
#else |
|
|
|
sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_AXIS]) |
|
|
|
|
Scott Lahteine
8 years ago
GitHub