Implement COREXZ in stepper.cpp and planner.cpp

This commit is contained in:
Scott Lahteine 2015-06-15 17:36:41 -07:00 committed by Richard Wackerbarth
parent fa00e1d97f
commit 9f53e2f0c9
2 changed files with 118 additions and 72 deletions

View File

@ -542,6 +542,11 @@ float junction_deviation = 0.1;
block->steps[A_AXIS] = labs(dx + dy); block->steps[A_AXIS] = labs(dx + dy);
block->steps[B_AXIS] = labs(dx - dy); block->steps[B_AXIS] = labs(dx - dy);
block->steps[Z_AXIS] = labs(dz); block->steps[Z_AXIS] = labs(dz);
#elif defined(COREXZ)
// corexz planning
block->steps[A_AXIS] = labs(dx + dz);
block->steps[Y_AXIS] = labs(dy);
block->steps[C_AXIS] = labs(dx - 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);
@ -572,6 +577,12 @@ float junction_deviation = 0.1;
if (dz < 0) db |= BIT(Z_AXIS); if (dz < 0) db |= BIT(Z_AXIS);
if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction
if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction
#elif defined(COREXZ)
if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) db |= BIT(Y_AXIS);
if (dz < 0) db |= BIT(Z_HEAD); // ...and Z
if (dx + dz < 0) db |= BIT(A_AXIS); // Motor A direction
if (dx - dz < 0) db |= BIT(C_AXIS); // Motor B direction
#else #else
if (dx < 0) db |= BIT(X_AXIS); if (dx < 0) db |= BIT(X_AXIS);
if (dy < 0) db |= BIT(Y_AXIS); if (dy < 0) db |= BIT(Y_AXIS);
@ -591,6 +602,11 @@ float junction_deviation = 0.1;
#ifndef Z_LATE_ENABLE #ifndef Z_LATE_ENABLE
if (block->steps[Z_AXIS]) enable_z(); if (block->steps[Z_AXIS]) enable_z();
#endif #endif
#elif defined(COREXZ)
if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
enable_x();
enable_z();
}
#else #else
if (block->steps[X_AXIS]) enable_x(); if (block->steps[X_AXIS]) enable_x();
if (block->steps[Y_AXIS]) enable_y(); if (block->steps[Y_AXIS]) enable_y();
@ -683,6 +699,13 @@ float junction_deviation = 0.1;
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS]; delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS]; delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS]; delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
#elif defined(COREXZ)
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_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[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS];
delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS];
#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];
@ -698,6 +721,8 @@ float junction_deviation = 0.1;
block->millimeters = sqrt( block->millimeters = sqrt(
#ifdef COREXY #ifdef COREXY
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 defined(COREXZ)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + 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

@ -342,34 +342,38 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
return timer; return timer;
} }
// 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
* X_AXIS=A_AXIS and Z_AXIS=C_AXIS for COREXZ
*/
void set_stepper_direction() { void set_stepper_direction() {
// Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY) if (TEST(out_bits, X_AXIS)) { // A_AXIS
if (TEST(out_bits, X_AXIS)) { X_APPLY_DIR(INVERT_X_DIR, 0);
X_APPLY_DIR(INVERT_X_DIR,0);
count_direction[X_AXIS] = -1; count_direction[X_AXIS] = -1;
} }
else { else {
X_APPLY_DIR(!INVERT_X_DIR,0); X_APPLY_DIR(!INVERT_X_DIR, 0);
count_direction[X_AXIS] = 1; count_direction[X_AXIS] = 1;
} }
if (TEST(out_bits, Y_AXIS)) { if (TEST(out_bits, Y_AXIS)) { // B_AXIS
Y_APPLY_DIR(INVERT_Y_DIR,0); Y_APPLY_DIR(INVERT_Y_DIR, 0);
count_direction[Y_AXIS] = -1; count_direction[Y_AXIS] = -1;
} }
else { else {
Y_APPLY_DIR(!INVERT_Y_DIR,0); Y_APPLY_DIR(!INVERT_Y_DIR, 0);
count_direction[Y_AXIS] = 1; count_direction[Y_AXIS] = 1;
} }
if (TEST(out_bits, Z_AXIS)) { if (TEST(out_bits, Z_AXIS)) { // C_AXIS
Z_APPLY_DIR(INVERT_Z_DIR,0); Z_APPLY_DIR(INVERT_Z_DIR, 0);
count_direction[Z_AXIS] = -1; count_direction[Z_AXIS] = -1;
} }
else { else {
Z_APPLY_DIR(!INVERT_Z_DIR,0); Z_APPLY_DIR(!INVERT_Z_DIR, 0);
count_direction[Z_AXIS] = 1; count_direction[Z_AXIS] = 1;
} }
@ -503,6 +507,11 @@ ISR(TIMER1_COMPA_vect) {
// If DeltaX == -DeltaY, the movement is only in Y axis // If DeltaX == -DeltaY, the movement is only in Y axis
if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, B_AXIS))) { if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, B_AXIS))) {
if (TEST(out_bits, X_HEAD)) if (TEST(out_bits, X_HEAD))
#elif defined(COREXZ)
// Head direction in -X axis for CoreXZ bots.
// If DeltaX == -DeltaZ, the movement is only in Z axis
if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, C_AXIS))) {
if (TEST(out_bits, X_HEAD))
#else #else
if (TEST(out_bits, X_AXIS)) // stepping along -X axis (regular Cartesian bot) if (TEST(out_bits, X_AXIS)) // stepping along -X axis (regular Cartesian bot)
#endif #endif
@ -528,8 +537,11 @@ ISR(TIMER1_COMPA_vect) {
#endif #endif
} }
} }
#ifdef COREXY #if defined(COREXY) || defined(COREXZ)
} }
#endif
#ifdef COREXY
// Head direction in -Y axis for CoreXY bots. // Head direction in -Y axis for CoreXY bots.
// If DeltaX == DeltaY, the movement is only in X axis // If DeltaX == DeltaY, the movement is only in X axis
if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) != TEST(out_bits, B_AXIS))) { if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) != TEST(out_bits, B_AXIS))) {
@ -547,82 +559,91 @@ ISR(TIMER1_COMPA_vect) {
UPDATE_ENDSTOP(Y, MAX); UPDATE_ENDSTOP(Y, MAX);
#endif #endif
} }
#ifdef COREXY #if defined(COREXY) || defined(COREXZ)
} }
#endif #endif
if (TEST(out_bits, Z_AXIS)) { // z -direction
#if HAS_Z_MIN
#ifdef Z_DUAL_ENDSTOPS #ifdef COREXZ
SET_ENDSTOP_BIT(Z, MIN); // Head direction in -Z axis for CoreXZ bots.
#if HAS_Z2_MIN // If DeltaX == DeltaZ, the movement is only in X axis
SET_ENDSTOP_BIT(Z2, MIN); if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (TEST(out_bits, A_AXIS) != TEST(out_bits, C_AXIS))) {
#else if (TEST(out_bits, Z_HEAD))
COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN); #else
#endif if (TEST(out_bits, Z_AXIS))
#endif
{ // z -direction
#if HAS_Z_MIN
byte z_test = TEST_ENDSTOP(Z_MIN) << 0 + TEST_ENDSTOP(Z2_MIN) << 1; // bit 0 for Z, bit 1 for Z2 #ifdef Z_DUAL_ENDSTOPS
SET_ENDSTOP_BIT(Z, MIN);
#if HAS_Z2_MIN
SET_ENDSTOP_BIT(Z2, MIN);
#else
COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
#endif
if (z_test && current_block->steps[Z_AXIS] > 0) { // z_test = Z_MIN || Z2_MIN byte z_test = TEST_ENDSTOP(Z_MIN) << 0 + TEST_ENDSTOP(Z2_MIN) << 1; // bit 0 for Z, bit 1 for Z2
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_hit_bits |= BIT(Z_MIN);
if (!performing_homing || (z_test == 0x3)) //if not performing home or if both endstops were trigged during homing...
step_events_completed = current_block->step_event_count;
}
#else // !Z_DUAL_ENDSTOPS
UPDATE_ENDSTOP(Z, MIN); if (z_test && current_block->steps[Z_AXIS] > 0) { // z_test = Z_MIN || Z2_MIN
#endif // !Z_DUAL_ENDSTOPS endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
#endif // Z_MIN_PIN endstop_hit_bits |= BIT(Z_MIN);
if (!performing_homing || (z_test == 0x3)) //if not performing home or if both endstops were trigged during homing...
step_events_completed = current_block->step_event_count;
}
#else // !Z_DUAL_ENDSTOPS
#ifdef Z_PROBE_ENDSTOP UPDATE_ENDSTOP(Z, MIN);
UPDATE_ENDSTOP(Z, PROBE); #endif // !Z_DUAL_ENDSTOPS
#endif // Z_MIN_PIN
if (TEST_ENDSTOP(Z_PROBE)) #ifdef Z_PROBE_ENDSTOP
{ UPDATE_ENDSTOP(Z, PROBE);
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_hit_bits |= BIT(Z_PROBE); if (TEST_ENDSTOP(Z_PROBE))
{
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_hit_bits |= BIT(Z_PROBE);
}
#endif
} }
#endif else { // z +direction
} #if HAS_Z_MAX
else { // z +direction
#if HAS_Z_MAX
#ifdef Z_DUAL_ENDSTOPS #ifdef Z_DUAL_ENDSTOPS
SET_ENDSTOP_BIT(Z, MAX); SET_ENDSTOP_BIT(Z, MAX);
#if HAS_Z2_MAX #if HAS_Z2_MAX
SET_ENDSTOP_BIT(Z2, MAX); SET_ENDSTOP_BIT(Z2, MAX);
#else #else
COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX) COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX)
#endif #endif
byte z_test = TEST_ENDSTOP(Z_MAX) << 0 + TEST_ENDSTOP(Z2_MAX) << 1; // bit 0 for Z, bit 1 for Z2 byte z_test = TEST_ENDSTOP(Z_MAX) << 0 + TEST_ENDSTOP(Z2_MAX) << 1; // bit 0 for Z, bit 1 for Z2
if (z_test && current_block->steps[Z_AXIS] > 0) { // t_test = Z_MAX || Z2_MAX if (z_test && current_block->steps[Z_AXIS] > 0) { // t_test = Z_MAX || Z2_MAX
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_hit_bits |= BIT(Z_MIN); endstop_hit_bits |= BIT(Z_MIN);
if (!performing_homing || (z_test == 0x3)) //if not performing home or if both endstops were trigged during homing... if (!performing_homing || (z_test == 0x3)) //if not performing home or if both endstops were trigged during homing...
step_events_completed = current_block->step_event_count; step_events_completed = current_block->step_event_count;
} }
#else // !Z_DUAL_ENDSTOPS #else // !Z_DUAL_ENDSTOPS
UPDATE_ENDSTOP(Z, MAX); UPDATE_ENDSTOP(Z, MAX);
#endif // !Z_DUAL_ENDSTOPS #endif // !Z_DUAL_ENDSTOPS
#endif // Z_MAX_PIN #endif // Z_MAX_PIN
#ifdef Z_PROBE_ENDSTOP #ifdef Z_PROBE_ENDSTOP
UPDATE_ENDSTOP(Z, PROBE); UPDATE_ENDSTOP(Z, PROBE);
if (TEST_ENDSTOP(Z_PROBE)) if (TEST_ENDSTOP(Z_PROBE))
{ {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_hit_bits |= BIT(Z_PROBE); endstop_hit_bits |= BIT(Z_PROBE);
}
#endif
} }
#endif
}
old_endstop_bits = current_endstop_bits; old_endstop_bits = current_endstop_bits;
} }