COREYZ stepper, planner, endstop, babysteps

This commit is contained in:
Scott Lahteine 2016-05-20 13:27:49 -07:00
parent 6c7f4909b1
commit 5172d4ba40
5 changed files with 76 additions and 48 deletions

View File

@ -239,8 +239,8 @@ void Endstops::update() {
#if ENABLED(COREXY) || ENABLED(COREXZ) #if ENABLED(COREXY) || ENABLED(COREXZ)
// Head direction in -X axis for CoreXY and CoreXZ bots. // Head direction in -X axis for CoreXY and CoreXZ bots.
// If Delta1 == -Delta2, the movement is only in Y or Z axis // If DeltaA == -DeltaB, the movement is only in Y or Z axis
if ((stepper.current_block->steps[A_AXIS] != stepper.current_block->steps[CORE_AXIS_2]) || (stepper.motor_direction(A_AXIS) == stepper.motor_direction(CORE_AXIS_2))) { if ((stepper.current_block->steps[CORE_AXIS_1] != stepper.current_block->steps[CORE_AXIS_2]) || (stepper.motor_direction(CORE_AXIS_1) == stepper.motor_direction(CORE_AXIS_2))) {
if (stepper.motor_direction(X_HEAD)) if (stepper.motor_direction(X_HEAD))
#else #else
if (stepper.motor_direction(X_AXIS)) // stepping along -X axis (regular Cartesian bot) if (stepper.motor_direction(X_AXIS)) // stepping along -X axis (regular Cartesian bot)
@ -271,10 +271,10 @@ void Endstops::update() {
} }
#endif #endif
#if ENABLED(COREXY) #if ENABLED(COREXY) || ENABLED(COREYZ)
// Head direction in -Y axis for CoreXY bots. // Head direction in -Y axis for CoreXY / CoreYZ bots.
// If DeltaX == DeltaY, the movement is only in X axis // If DeltaA == DeltaB, the movement is only in X or Y axis
if ((stepper.current_block->steps[A_AXIS] != stepper.current_block->steps[B_AXIS]) || (stepper.motor_direction(A_AXIS) != stepper.motor_direction(B_AXIS))) { if ((stepper.current_block->steps[CORE_AXIS_1] != stepper.current_block->steps[CORE_AXIS_2]) || (stepper.motor_direction(CORE_AXIS_1) != stepper.motor_direction(CORE_AXIS_2))) {
if (stepper.motor_direction(Y_HEAD)) if (stepper.motor_direction(Y_HEAD))
#else #else
if (stepper.motor_direction(Y_AXIS)) // -direction if (stepper.motor_direction(Y_AXIS)) // -direction
@ -289,14 +289,14 @@ void Endstops::update() {
UPDATE_ENDSTOP(Y, MAX); UPDATE_ENDSTOP(Y, MAX);
#endif #endif
} }
#if ENABLED(COREXY) #if ENABLED(COREXY) || ENABLED(COREYZ)
} }
#endif #endif
#if ENABLED(COREXZ) #if ENABLED(COREXZ) || ENABLED(COREYZ)
// Head direction in -Z axis for CoreXZ bots. // Head direction in -Z axis for CoreXZ or CoreYZ bots.
// If DeltaX == DeltaZ, the movement is only in X axis // If DeltaA == DeltaB, the movement is only in X or Y axis
if ((stepper.current_block->steps[A_AXIS] != stepper.current_block->steps[C_AXIS]) || (stepper.motor_direction(A_AXIS) != stepper.motor_direction(C_AXIS))) { if ((stepper.current_block->steps[CORE_AXIS_1] != stepper.current_block->steps[CORE_AXIS_2]) || (stepper.motor_direction(CORE_AXIS_1) != stepper.motor_direction(CORE_AXIS_2))) {
if (stepper.motor_direction(Z_HEAD)) if (stepper.motor_direction(Z_HEAD))
#else #else
if (stepper.motor_direction(Z_AXIS)) if (stepper.motor_direction(Z_AXIS))

View File

@ -543,6 +543,11 @@ void Planner::check_axes_activity() {
block->steps[A_AXIS] = labs(dx + dz); block->steps[A_AXIS] = labs(dx + dz);
block->steps[Y_AXIS] = labs(dy); block->steps[Y_AXIS] = labs(dy);
block->steps[C_AXIS] = labs(dx - dz); block->steps[C_AXIS] = labs(dx - dz);
#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);
#else #else
// default non-h-bot planning // default non-h-bot planning
block->steps[X_AXIS] = labs(dx); block->steps[X_AXIS] = labs(dx);
@ -581,7 +586,13 @@ void Planner::check_axes_activity() {
if (dy < 0) SBI(db, Y_AXIS); if (dy < 0) SBI(db, Y_AXIS);
if (dz < 0) SBI(db, Z_HEAD); // ...and Z 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, A_AXIS); // Motor A direction
if (dx - dz < 0) SBI(db, C_AXIS); // Motor B direction if (dx - dz < 0) SBI(db, 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
#else #else
if (dx < 0) SBI(db, X_AXIS); if (dx < 0) SBI(db, X_AXIS);
if (dy < 0) SBI(db, Y_AXIS); if (dy < 0) SBI(db, Y_AXIS);
@ -698,20 +709,27 @@ void Planner::check_axes_activity() {
* 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) #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
float delta_mm[6]; float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS]; #if ENABLED(COREXY)
delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS]; delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS]; delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS]; delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS]; delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
#elif ENABLED(COREXZ) delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
float delta_mm[6]; #elif ENABLED(COREXZ)
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS]; delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS]; delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS]; delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS]; delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS];
delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS]; delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS];
#elif ENABLED(COREYZ)
delta_mm[X_AXIS] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_HEAD] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
delta_mm[B_AXIS] = (dy + dz) / axis_steps_per_unit[B_AXIS];
delta_mm[C_AXIS] = (dy - dz) / axis_steps_per_unit[C_AXIS];
#endif
#else #else
float delta_mm[4]; float delta_mm[4];
delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS]; delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
@ -729,6 +747,8 @@ void Planner::check_axes_activity() {
square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS]) square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD]) square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD])
#elif ENABLED(COREYZ)
square(delta_mm[X_AXIS]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_HEAD])
#else #else
square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]) square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
#endif #endif

View File

@ -204,7 +204,7 @@ class Planner {
* Used by G92, G28, G29, and other procedures. * Used by G92, G28, G29, and other procedures.
* *
* Multiplies by axis_steps_per_unit[] and does necessary conversion * Multiplies by axis_steps_per_unit[] and does necessary conversion
* for COREXY / COREXZ to set the corresponding stepper positions. * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
* *
* Clears previous speed values. * Clears previous speed values.
*/ */

View File

@ -204,8 +204,9 @@ void Stepper::wake_up() {
/** /**
* Set the stepper direction of each axis * Set the stepper direction of each axis
* *
* X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY * COREXY: X_AXIS=A_AXIS and Y_AXIS=B_AXIS
* X_AXIS=A_AXIS and Z_AXIS=C_AXIS for COREXZ * COREXZ: X_AXIS=A_AXIS and Z_AXIS=C_AXIS
* COREYZ: Y_AXIS=B_AXIS and Z_AXIS=C_AXIS
*/ */
void Stepper::set_directions() { void Stepper::set_directions() {
@ -649,6 +650,11 @@ void Stepper::set_position(const long& x, const long& y, const long& z, const lo
count_position[A_AXIS] = x + z; count_position[A_AXIS] = x + z;
count_position[Y_AXIS] = y; count_position[Y_AXIS] = y;
count_position[C_AXIS] = x - z; count_position[C_AXIS] = x - z;
#elif ENABLED(COREYZ)
// coreyz planning
count_position[X_AXIS] = x;
count_position[B_AXIS] = y + z;
count_position[C_AXIS] = y - z;
#else #else
// default non-h-bot planning // default non-h-bot planning
count_position[X_AXIS] = x; count_position[X_AXIS] = x;
@ -682,15 +688,16 @@ long Stepper::position(AxisEnum axis) {
*/ */
float Stepper::get_axis_position_mm(AxisEnum axis) { float Stepper::get_axis_position_mm(AxisEnum axis) {
float axis_steps; float axis_steps;
#if ENABLED(COREXY) | ENABLED(COREXZ) #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
if (axis == X_AXIS || axis == CORE_AXIS_2) { // Requesting one of the "core" axes?
if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
long pos1 = count_position[A_AXIS], long pos1 = count_position[CORE_AXIS_1],
pos2 = count_position[CORE_AXIS_2]; pos2 = count_position[CORE_AXIS_2];
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
// ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1 // ((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 // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
axis_steps = (pos1 + ((axis == X_AXIS) ? pos2 : -pos2)) / 2.0f; axis_steps = (pos1 + ((axis == CORE_AXIS_1) ? pos2 : -pos2)) / 2.0f;
} }
else else
axis_steps = position(axis); axis_steps = position(axis);
@ -715,20 +722,20 @@ void Stepper::quick_stop() {
void Stepper::endstop_triggered(AxisEnum axis) { void Stepper::endstop_triggered(AxisEnum axis) {
#if ENABLED(COREXY) || ENABLED(COREXZ) #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
float axis_pos = count_position[axis]; float axis_pos = count_position[axis];
if (axis == A_AXIS) if (axis == CORE_AXIS_1)
axis_pos = (axis_pos + count_position[CORE_AXIS_2]) / 2; axis_pos = (axis_pos + count_position[CORE_AXIS_2]) / 2;
else if (axis == CORE_AXIS_2) else if (axis == CORE_AXIS_2)
axis_pos = (count_position[A_AXIS] - axis_pos) / 2; axis_pos = (count_position[CORE_AXIS_1] - axis_pos) / 2;
endstops_trigsteps[axis] = axis_pos; endstops_trigsteps[axis] = axis_pos;
#else // !COREXY && !COREXZ #else // !COREXY && !COREXZ && !COREYZ
endstops_trigsteps[axis] = count_position[axis]; endstops_trigsteps[axis] = count_position[axis];
#endif // !COREXY && !COREXZ #endif // !COREXY && !COREXZ && !COREYZ
kill_current_block(); kill_current_block();
} }
@ -747,14 +754,14 @@ void Stepper::report_positions() {
#endif #endif
SERIAL_PROTOCOL(xpos); SERIAL_PROTOCOL(xpos);
#if ENABLED(COREXY) || ENABLED(COREXZ) #if ENABLED(COREXY) || ENABLED(COREYZ)
SERIAL_PROTOCOLPGM(" B:"); SERIAL_PROTOCOLPGM(" B:");
#else #else
SERIAL_PROTOCOLPGM(" Y:"); SERIAL_PROTOCOLPGM(" Y:");
#endif #endif
SERIAL_PROTOCOL(ypos); SERIAL_PROTOCOL(ypos);
#if ENABLED(COREXZ) || ENABLED(COREXZ) #if ENABLED(COREXZ) || ENABLED(COREYZ)
SERIAL_PROTOCOLPGM(" C:"); SERIAL_PROTOCOLPGM(" C:");
#else #else
SERIAL_PROTOCOLPGM(" Z:"); SERIAL_PROTOCOLPGM(" Z:");

View File

@ -306,24 +306,25 @@ class Temperature {
#if ENABLED(BABYSTEPPING) #if ENABLED(BABYSTEPPING)
FORCE_INLINE void babystep_axis(AxisEnum axis, int distance) { FORCE_INLINE void babystep_axis(AxisEnum axis, int distance) {
#if ENABLED(COREXY) || ENABLED(COREXZ) #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
#if ENABLED(BABYSTEP_XY) #if ENABLED(BABYSTEP_XY)
switch (axis) { switch (axis) {
case X_AXIS: // X on CoreXY and CoreXZ case CORE_AXIS_1: // X on CoreXY and CoreXZ, Y on CoreYZ
babystepsTodo[A_AXIS] += distance * 2; babystepsTodo[CORE_AXIS_1] += distance * 2;
babystepsTodo[CORE_AXIS_2] += distance * 2; babystepsTodo[CORE_AXIS_2] += distance * 2;
break; break;
case CORE_AXIS_2: // Y on CoreXY, Z on CoreXZ case CORE_AXIS_2: // Y on CoreXY, Z on CoreXZ and CoreYZ
babystepsTodo[A_AXIS] += distance * 2; babystepsTodo[CORE_AXIS_1] += distance * 2;
babystepsTodo[CORE_AXIS_2] -= distance * 2; babystepsTodo[CORE_AXIS_2] -= distance * 2;
break; break;
case CORE_AXIS_3: // Z on CoreXY, Y on CoreXZ case CORE_AXIS_3: // Z on CoreXY, Y on CoreXZ, X on CoreYZ
babystepsTodo[CORE_AXIS_3] += distance; babystepsTodo[CORE_AXIS_3] += distance;
break; break;
} }
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ) || ENABLED(COREYZ)
babystepsTodo[A_AXIS] += distance * 2; // Only Z stepping needs to be handled here
babystepsTodo[C_AXIS] -= distance * 2; babystepsTodo[CORE_AXIS_1] += distance * 2;
babystepsTodo[CORE_AXIS_2] -= distance * 2;
#else #else
babystepsTodo[Z_AXIS] += distance; babystepsTodo[Z_AXIS] += distance;
#endif #endif