From 45124316e33ef0182e32bfd5dedac914ebed4262 Mon Sep 17 00:00:00 2001 From: Ben Gamari Date: Fri, 16 Sep 2011 18:45:20 -0400 Subject: [PATCH] Rework endstop homing. The DDA is now used for motion control. Note from Traumflug: thanks a lot for this excellent patch, Ben. --- dda.c | 83 ++++++++++++++++++-- dda.h | 8 ++ dda_queue.c | 6 ++ dda_queue.h | 1 + home.c | 212 +++++++++++++--------------------------------------- 5 files changed, 142 insertions(+), 168 deletions(-) diff --git a/dda.c b/dda.c index f1e2143..7977a3f 100644 --- a/dda.c +++ b/dda.c @@ -451,9 +451,33 @@ void dda_start(DDA *dda) { */ void dda_step(DDA *dda) { uint8_t did_step = 0; + uint8_t endstop_stop; ///< Stop due to endstop trigger + uint8_t endstop_not_done = 0; ///< Which axes haven't finished homing - if ((move_state.x_steps) /* && - (x_max() != dda->x_direction) && (x_min() == dda->x_direction) */) { +#if defined X_MIN_PIN || defined X_MAX_PIN + if (dda->endstop_check & 0x1) { +#if defined X_MIN_PIN + if (x_min() == dda->endstop_stop_cond) + move_state.debounce_count_xmin++; + else + move_state.debounce_count_xmin = 0; +#endif + +#if defined X_MAX_PIN + if (x_max() == dda->endstop_stop_cond) + move_state.debounce_count_xmax++; + else + move_state.debounce_count_xmax = 0; +#endif + + endstop_stop = move_state.debounce_count_xmin >= 8 || move_state.debounce_count_xmax >= 8; + if (!endstop_stop) + endstop_not_done |= 0x1; + } else +#endif + endstop_stop = 0; + + if ((move_state.x_steps) && !endstop_stop) { move_state.x_counter -= dda->x_delta; if (move_state.x_counter < 0) { x_step(); @@ -463,8 +487,30 @@ void dda_step(DDA *dda) { } } - if ((move_state.y_steps) /* && - (y_max() != dda->y_direction) && (y_min() == dda->y_direction) */) { +#if defined Y_MIN_PIN || defined Y_MAX_PIN + if (dda->endstop_check & 0x2) { +#if defined Y_MIN_PIN + if (y_min() == dda->endstop_stop_cond) + move_state.debounce_count_ymin++; + else + move_state.debounce_count_ymin = 0; +#endif + +#if defined Y_MAX_PIN + if (y_max() == dda->endstop_stop_cond) + move_state.debounce_count_ymax++; + else + move_state.debounce_count_ymax = 0; +#endif + + endstop_stop = move_state.debounce_count_ymin >= 8 || move_state.debounce_count_ymax >= 8; + if (!endstop_stop) + endstop_not_done |= 0x2; + } else +#endif + endstop_stop = 0; + + if ((move_state.y_steps) && !endstop_stop) { move_state.y_counter -= dda->y_delta; if (move_state.y_counter < 0) { y_step(); @@ -474,8 +520,30 @@ void dda_step(DDA *dda) { } } - if ((move_state.z_steps) /* && - (z_max() != dda->z_direction) && (z_min() == dda->z_direction) */) { +#if defined Z_MIN_PIN || defined Z_MAX_PIN + if (dda->endstop_check & 0x4) { +#if defined Z_MIN_PIN + if (z_min() == dda->endstop_stop_cond) + move_state.debounce_count_zmin++; + else + move_state.debounce_count_zmin = 0; +#endif + +#if defined Z_MAX_PIN + if (z_max() == dda->endstop_stop_cond) + move_state.debounce_count_zmax++; + else + move_state.debounce_count_zmax = 0; +#endif + + endstop_stop = move_state.debounce_count_zmin >= 8 || move_state.debounce_count_zmax >= 8; + if (!endstop_stop) + endstop_not_done |= 0x4; + } else +#endif + endstop_stop = 0; + + if ((move_state.z_steps) && !endstop_stop) { move_state.z_counter -= dda->z_delta; if (move_state.z_counter < 0) { z_step(); @@ -564,6 +632,9 @@ void dda_step(DDA *dda) { // sersendf_P(PSTR("\r\nc %lu c_min %lu n %d"), dda->c, dda->c_min, move_state.n); #endif + if (dda->endstop_check != 0x0 && endstop_not_done == 0x0) + move_state.x_steps = move_state.y_steps = move_state.z_steps = move_state.e_steps = 0; + // TODO: did_step is obsolete ... if (did_step) { // we stepped, reset timeout diff --git a/dda.h b/dda.h index bad6ce4..41adbd4 100644 --- a/dda.h +++ b/dda.h @@ -61,6 +61,10 @@ typedef struct { /// tracking variable int16_t n; #endif + + /// Endstop debouncing + uint8_t debounce_count_xmin, debounce_count_ymin, debounce_count_zmin; + uint8_t debounce_count_xmax, debounce_count_ymax, debounce_count_zmax; } MOVE_STATE; /** @@ -118,6 +122,10 @@ typedef struct { /// 24.8 fixed point timer value, maximum speed uint32_t c_min; #endif + + /// Endstop homing + uint8_t endstop_check; ///< Do we need to check endstops? 0x1=Check X, 0x2=Check Y, 0x4=Check Z + uint8_t endstop_stop_cond; ///< Endstop condition on which to stop motion: 0=Stop on detrigger, 1=Stop on trigger } DDA; /* diff --git a/dda_queue.c b/dda_queue.c index a0b50f7..dc868ac 100644 --- a/dda_queue.c +++ b/dda_queue.c @@ -95,6 +95,10 @@ void queue_step() { /// \note this function waits for space to be available if necessary, check queue_full() first if waiting is a problem /// This is the only function that modifies mb_head and it always called from outside an interrupt. void enqueue(TARGET *t) { + enqueue_home(t, 0, 0); +} + +void enqueue_home(TARGET *t, uint8_t endstop_check, uint8_t endstop_stop_cond) { // don't call this function when the queue is full, but just in case, wait for a move to complete and free up the space for the passed target while (queue_full()) delay(WAITING_DELAY); @@ -106,6 +110,8 @@ void enqueue(TARGET *t) { if (t != NULL) { dda_create(new_movebuffer, t); + new_movebuffer->endstop_check = endstop_check; + new_movebuffer->endstop_stop_cond = endstop_stop_cond; } else { // it's a wait for temp diff --git a/dda_queue.h b/dda_queue.h index b676fae..15fb71b 100644 --- a/dda_queue.h +++ b/dda_queue.h @@ -29,6 +29,7 @@ void queue_step(void); // add a new target to the queue // t == NULL means add a wait for target temp to the queue void enqueue(TARGET *t); +void enqueue_home(TARGET *t, uint8_t endstop_check, uint8_t endstop_stop_cond); // called from step timer when current move is complete void next_move(void) __attribute__ ((hot)); diff --git a/home.c b/home.c index b838c30..03dfa37 100644 --- a/home.c +++ b/home.c @@ -32,41 +32,22 @@ void home() { /// find X MIN endstop void home_x_negative() { + TARGET t = {0, current_position.Y, current_position.Z}; power_on(); queue_wait(); stepper_enable(); x_enable(); #if defined X_MIN_PIN - uint8_t debounce_count = 0; - // hit home hard - x_direction(0); - while (debounce_count < 8) { - // debounce - if (x_min()) - debounce_count++; - else - debounce_count = 0; - // step - x_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_X / ((float) MAXIMUM_FEEDRATE_X))); - } - debounce_count = 0; + t.X = -1000*STEPS_PER_MM_X; + t.F = MAXIMUM_FEEDRATE_X; + enqueue_home(&t, 0x1, 1); // back off slowly - x_direction(1); - while (x_min() != 0) { - // step - x_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_X / ((float) SEARCH_FEEDRATE_X))); - } + t.X = +1000*STEPS_PER_MM_X; + t.F = SEARCH_FEEDRATE_X; + enqueue_home(&t, 0x1, 0); // set X home #ifdef X_MIN @@ -79,88 +60,51 @@ void home_x_negative() { /// find X_MAX endstop void home_x_positive() { + TARGET t = {0, current_position.Y, current_position.Z}; power_on(); queue_wait(); stepper_enable(); x_enable(); #if defined X_MAX_PIN - uint8_t debounce_count = 0; - // hit home hard - x_direction(1); - while (debounce_count < 8) { - // debounce - if (x_max()) - debounce_count++; - else - debounce_count = 0; - // step - x_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_X / ((float) MAXIMUM_FEEDRATE_X))); - } - debounce_count = 0; + t.X = +1000*STEPS_PER_MM_X; + t.F = MAXIMUM_FEEDRATE_X; + enqueue_home(t, 0x1, 1); // back off slowly - x_direction(0); - while (x_max() != 0) { - // step - x_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_X / ((float) SEARCH_FEEDRATE_X))); - } + t.X = -1000*STEPS_PER_MM_X; + t.F = SEARCH_FEEDRATE_X; + enqueue_home(t, 0x1, 0); // set X home // set position to MAX startpoint.X = current_position.X = (int32_t) (X_MAX * STEPS_PER_MM_X); // go to zero - TARGET t = {0, 0, 0, 0, MAXIMUM_FEEDRATE_X}; + t.X = 0; + t.F = MAXIMUM_FEEDRATE_X; enqueue(&t); #endif } /// fund Y MIN endstop void home_y_negative() { + TARGET t = {0, current_position.Y, current_position.Z}; power_on(); queue_wait(); stepper_enable(); y_enable(); #if defined Y_MIN_PIN - uint8_t debounce_count = 0; - // hit home hard - y_direction(0); - while (debounce_count < 8) { - // debounce - if (y_min()) - debounce_count++; - else - debounce_count = 0; - // step - y_step(); - delay(5); - unstep(); - // wait until neyt step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Y / ((float) MAXIMUM_FEEDRATE_Y))); - } - debounce_count = 0; + t.Y = -1000*STEPS_PER_MM_Y; + t.F = MAXIMUM_FEEDRATE_Y; + enqueue_home(&t, 0x2, 1); // back off slowly - y_direction(1); - while (y_min() != 0) { - // step - y_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Y / ((float) SEARCH_FEEDRATE_Y))); - } + t.Y = +1000*STEPS_PER_MM_Y; + t.F = SEARCH_FEEDRATE_Y; + enqueue_home(&t, 0x2, 0); // set Y home #ifdef Y_MIN @@ -173,88 +117,51 @@ void home_y_negative() { /// find Y MAX endstop void home_y_positive() { + TARGET t = {0, current_position.Y, current_position.Z}; power_on(); queue_wait(); stepper_enable(); y_enable(); #if defined Y_MAX_PIN - uint8_t debounce_count = 0; - // hit home hard - y_direction(1); - while (debounce_count < 8) { - // debounce - if (y_max()) - debounce_count++; - else - debounce_count = 0; - // step - y_step(); - delay(5); - unstep(); - // wait until neyt step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Y / ((float) MAXIMUM_FEEDRATE_Y))); - } - debounce_count = 0; + t.Y = +1000*STEPS_PER_MM_Y; + t.F = MAXIMUM_FEEDRATE_Y; + enqueue_home(&t, 0x2, 1); // back off slowly - y_direction(0); - while (y_max() != 0) { - // step - y_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Y / ((float) SEARCH_FEEDRATE_Y))); - } + t.X = -1000*STEPS_PER_MM_Y; + t.F = SEARCH_FEEDRATE_Y; + enqueue_home(&t, 0x2, 0); // set Y home // set position to MAX startpoint.Y = current_position.Y = (int32_t) (Y_MAX * STEPS_PER_MM_Y); // go to zero - TARGET t = {0, 0, 0, 0, MAXIMUM_FEEDRATE_Y}; + t.Y = 0; + t.F = MAXIMUM_FEEDRATE_Y; enqueue(&t); #endif } /// find Z MIN endstop void home_z_negative() { + TARGET t = {current_position.X, current_position.Y, 0}; power_on(); queue_wait(); stepper_enable(); z_enable(); #if defined Z_MIN_PIN - uint8_t debounce_count = 0; - // hit home hard - z_direction(0); - while (debounce_count < 8) { - // debounce - if (z_min()) - debounce_count++; - else - debounce_count = 0; - // step - z_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Z / ((float) MAXIMUM_FEEDRATE_Z))); - } - debounce_count = 0; + t.Z = -1000*STEPS_PER_MM_Z; + t.F = MAXIMUM_FEEDRATE_Z; + enqueue_home(&t, 0x4, 1); // back off slowly - z_direction(1); - while (z_min() != 0) { - // step - z_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Z / ((float) SEARCH_FEEDRATE_Z))); - } + t.Z = +1000*STEPS_PER_MM_Z; + t.F = SEARCH_FEEDRATE_Z; + enqueue_home(&t, 0x4, 0); // set Z home #ifdef Z_MIN @@ -268,48 +175,29 @@ void home_z_negative() { /// find Z MAX endstop void home_z_positive() { + TARGET t = {current_position.X, current_position.Y, 0}; power_on(); queue_wait(); stepper_enable(); z_enable(); #if defined Z_MAX_PIN - uint8_t debounce_count = 0; - // hit home hard - z_direction(1); - while (debounce_count < 8) { - // debounce - if (z_max()) - debounce_count++; - else - debounce_count = 0; - // step - z_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Z / ((float) MAXIMUM_FEEDRATE_Z))); - } - debounce_count = 0; + t.Z = +1000*STEPS_PER_MM_Z; + t.F = MAXIMUM_FEEDRATE_Z; + enqueue_home(&t, 0x4, 1); // back off slowly - z_direction(0); - while (z_max() != 0) { - // step - z_step(); - delay(5); - unstep(); - // wait until next step time - delay((uint32_t) (60.0 * 1000000.0 / STEPS_PER_MM_Z / ((float) SEARCH_FEEDRATE_Z))); - } + t.Z = -1000*STEPS_PER_MM_Z; + t.F = SEARCH_FEEDRATE_Z; + enqueue_home(&t, 0x4, 0); - // set Z home: + // set Z home // set position to MAX startpoint.Z = current_position.Z = (int32_t) (Z_MAX * STEPS_PER_MM_Z); - // go to zero - // TARGET t = {0, 0, 0, 0, MAXIMUM_FEEDRATE_Z}; - // enqueue(&t); + t.Z = 0; + t.F = MAXIMUM_FEEDRATE_Z; + enqueue(&t); #endif }