|
|
|
@ -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
|
|
|
|
// Compute direction bit-mask for this block
|
|
|
|
uint8_t dm = 0;
|
|
|
|
uint8_t dm = 0;
|
|
|
|
#if ENABLED(COREXY)
|
|
|
|
#if CORE_IS_XY
|
|
|
|
if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
|
|
|
|
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 (db < 0) SBI(dm, Y_HEAD); // ...and Y
|
|
|
|
if (dc < 0) SBI(dm, Z_AXIS);
|
|
|
|
if (dc < 0) SBI(dm, Z_AXIS);
|
|
|
|
if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction
|
|
|
|
if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction
|
|
|
|
if (da - db < 0) SBI(dm, B_AXIS); // Motor B direction
|
|
|
|
if (CORESIGN(da - db) < 0) SBI(dm, B_AXIS); // Motor B direction
|
|
|
|
#elif ENABLED(COREXZ)
|
|
|
|
#elif CORE_IS_XZ
|
|
|
|
if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
|
|
|
|
if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
|
|
|
|
if (db < 0) SBI(dm, Y_AXIS);
|
|
|
|
if (db < 0) SBI(dm, Y_AXIS);
|
|
|
|
if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
|
|
|
|
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, A_AXIS); // Motor A direction
|
|
|
|
if (da - dc < 0) SBI(dm, C_AXIS); // Motor C direction
|
|
|
|
if (CORESIGN(da - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
|
|
|
|
#elif ENABLED(COREYZ)
|
|
|
|
#elif CORE_IS_YZ
|
|
|
|
if (da < 0) SBI(dm, X_AXIS);
|
|
|
|
if (da < 0) SBI(dm, X_AXIS);
|
|
|
|
if (db < 0) SBI(dm, Y_HEAD); // Save the real Extruder (head) direction in Y 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 (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, B_AXIS); // Motor B direction
|
|
|
|
if (db - dc < 0) SBI(dm, C_AXIS); // Motor C direction
|
|
|
|
if (CORESIGN(db - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
|
|
|
|
#else
|
|
|
|
#else
|
|
|
|
if (da < 0) SBI(dm, X_AXIS);
|
|
|
|
if (da < 0) SBI(dm, X_AXIS);
|
|
|
|
if (db < 0) SBI(dm, Y_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;
|
|
|
|
block->direction_bits = dm;
|
|
|
|
|
|
|
|
|
|
|
|
// Number of steps for each axis
|
|
|
|
// Number of steps for each axis
|
|
|
|
#if ENABLED(COREXY)
|
|
|
|
// See http://www.corexy.com/theory.html
|
|
|
|
// corexy planning
|
|
|
|
#if CORE_IS_XY
|
|
|
|
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
|
|
|
|
|
|
|
|
block->steps[A_AXIS] = labs(da + db);
|
|
|
|
block->steps[A_AXIS] = labs(da + db);
|
|
|
|
block->steps[B_AXIS] = labs(da - db);
|
|
|
|
block->steps[B_AXIS] = labs(da - db);
|
|
|
|
block->steps[Z_AXIS] = labs(dc);
|
|
|
|
block->steps[Z_AXIS] = labs(dc);
|
|
|
|
#elif ENABLED(COREXZ)
|
|
|
|
#elif CORE_IS_XZ
|
|
|
|
// corexz planning
|
|
|
|
|
|
|
|
block->steps[A_AXIS] = labs(da + dc);
|
|
|
|
block->steps[A_AXIS] = labs(da + dc);
|
|
|
|
block->steps[Y_AXIS] = labs(db);
|
|
|
|
block->steps[Y_AXIS] = labs(db);
|
|
|
|
block->steps[C_AXIS] = labs(da - dc);
|
|
|
|
block->steps[C_AXIS] = labs(da - dc);
|
|
|
|
#elif ENABLED(COREYZ)
|
|
|
|
#elif CORE_IS_YZ
|
|
|
|
// coreyz planning
|
|
|
|
|
|
|
|
block->steps[X_AXIS] = labs(da);
|
|
|
|
block->steps[X_AXIS] = labs(da);
|
|
|
|
block->steps[B_AXIS] = labs(db + dc);
|
|
|
|
block->steps[B_AXIS] = labs(db + dc);
|
|
|
|
block->steps[C_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;
|
|
|
|
block->active_extruder = extruder;
|
|
|
|
|
|
|
|
|
|
|
|
//enable active axes
|
|
|
|
//enable active axes
|
|
|
|
#if ENABLED(COREXY)
|
|
|
|
#if CORE_IS_XY
|
|
|
|
if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
|
|
|
|
if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
|
|
|
|
enable_x();
|
|
|
|
enable_x();
|
|
|
|
enable_y();
|
|
|
|
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 DISABLED(Z_LATE_ENABLE)
|
|
|
|
if (block->steps[Z_AXIS]) enable_z();
|
|
|
|
if (block->steps[Z_AXIS]) enable_z();
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
#elif ENABLED(COREXZ)
|
|
|
|
#elif CORE_IS_XZ
|
|
|
|
if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
|
|
|
|
if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
|
|
|
|
enable_x();
|
|
|
|
enable_x();
|
|
|
|
enable_z();
|
|
|
|
enable_z();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (block->steps[Y_AXIS]) enable_y();
|
|
|
|
if (block->steps[Y_AXIS]) enable_y();
|
|
|
|
#elif ENABLED(COREYZ)
|
|
|
|
#elif CORE_IS_YZ
|
|
|
|
if (block->steps[B_AXIS] || block->steps[C_AXIS]) {
|
|
|
|
if (block->steps[B_AXIS] || block->steps[C_AXIS]) {
|
|
|
|
enable_y();
|
|
|
|
enable_y();
|
|
|
|
enable_z();
|
|
|
|
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.
|
|
|
|
* 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.
|
|
|
|
* 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];
|
|
|
|
float delta_mm[7];
|
|
|
|
#if ENABLED(COREXY)
|
|
|
|
#if CORE_IS_XY
|
|
|
|
delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
|
|
|
|
delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
|
|
|
|
delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
|
|
|
|
delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
|
|
|
|
delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
|
|
|
|
delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
|
|
|
|
delta_mm[A_AXIS] = (da + db) * steps_to_mm[A_AXIS];
|
|
|
|
delta_mm[A_AXIS] = (da + db) * steps_to_mm[A_AXIS];
|
|
|
|
delta_mm[B_AXIS] = (da - db) * steps_to_mm[B_AXIS];
|
|
|
|
delta_mm[B_AXIS] = CORESIGN(da - db) * steps_to_mm[B_AXIS];
|
|
|
|
#elif ENABLED(COREXZ)
|
|
|
|
#elif CORE_IS_XZ
|
|
|
|
delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
|
|
|
|
delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
|
|
|
|
delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
|
|
|
|
delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
|
|
|
|
delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
|
|
|
|
delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
|
|
|
|
delta_mm[A_AXIS] = (da + dc) * steps_to_mm[A_AXIS];
|
|
|
|
delta_mm[A_AXIS] = (da + dc) * steps_to_mm[A_AXIS];
|
|
|
|
delta_mm[C_AXIS] = (da - dc) * steps_to_mm[C_AXIS];
|
|
|
|
delta_mm[C_AXIS] = CORESIGN(da - dc) * steps_to_mm[C_AXIS];
|
|
|
|
#elif ENABLED(COREYZ)
|
|
|
|
#elif CORE_IS_YZ
|
|
|
|
delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
|
|
|
|
delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
|
|
|
|
delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
|
|
|
|
delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
|
|
|
|
delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
|
|
|
|
delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
|
|
|
|
delta_mm[B_AXIS] = (db + dc) * steps_to_mm[B_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
|
|
|
|
#endif
|
|
|
|
#else
|
|
|
|
#else
|
|
|
|
float delta_mm[4];
|
|
|
|
float delta_mm[4];
|
|
|
|
@ -949,11 +946,11 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
else {
|
|
|
|
block->millimeters = sqrt(
|
|
|
|
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])
|
|
|
|
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])
|
|
|
|
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])
|
|
|
|
sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_HEAD])
|
|
|
|
#else
|
|
|
|
#else
|
|
|
|
sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_AXIS])
|
|
|
|
sq(delta_mm[X_AXIS]) + sq(delta_mm[Y_AXIS]) + sq(delta_mm[Z_AXIS])
|
|
|
|
|
Scott Lahteine
8 years ago
committed by