From 7a972fd9b07f3a6885ba06f5f7cc8f282361b50c Mon Sep 17 00:00:00 2001 From: bubnikv Date: Sun, 14 Jan 2018 22:37:07 +0100 Subject: [PATCH] Split the stepper ISR routine into multiple inline functions, added an optimized DDA routine for moves with less than 32767 ticks. --- Firmware/planner.cpp | 5 +- Firmware/planner.h | 4 +- Firmware/stepper.cpp | 635 ++++++++++++++++++++++--------------------- 3 files changed, 331 insertions(+), 313 deletions(-) diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index 5d0e703e0..41a4e8ea3 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -839,9 +839,7 @@ block->steps_y.wide = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-p if(block->steps_x.wide != 0) enable_x(); if(block->steps_y.wide != 0) enable_y(); #endif -#ifndef Z_LATE_ENABLE if(block->steps_z.wide != 0) enable_z(); -#endif // Enable extruder(s) if(block->steps_e.wide != 0) @@ -1234,6 +1232,9 @@ Having the real displacement of the head, we can calculate the total movement le block->speed_factor = block->nominal_rate / block->nominal_speed; calculate_trapezoid_for_block(block, block->entry_speed, safe_speed); + if (block->step_event_count.wide <= 32767) + block->flag |= BLOCK_FLAG_DDA_LOWRES; + // Move the buffer head. From now the block may be picked up by the stepper interrupt controller. block_buffer_head = next_buffer_head; diff --git a/Firmware/planner.h b/Firmware/planner.h index 91c5c5fa2..ecac73d18 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -50,7 +50,7 @@ union dda_isteps_t { int32_t wide; struct { - uint16_t lo; + int16_t lo; int16_t hi; }; }; @@ -94,7 +94,7 @@ typedef struct { float acceleration; // Bit flags defined by the BlockFlag enum. - bool flag; + uint8_t flag; // Settings for the trapezoid generator (runs inside an interrupt handler). // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts. diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index 4b53ee89e..bb020c44e 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -135,8 +135,6 @@ extern bool stepper_timer_overflow_state; //=============================functions ============================ //=========================================================================== -#define CHECK_ENDSTOPS if(check_endstops) - #ifndef _NO_ASM // intRes = intIn1 * intIn2 >> 16 @@ -320,7 +318,7 @@ void step_wait(){ } -FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) { +FORCE_INLINE unsigned short calc_timer(uint16_t step_rate) { unsigned short timer; if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY; @@ -361,10 +359,10 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) { FORCE_INLINE void trapezoid_generator_reset() { deceleration_time = 0; // step_rate to timer interval - OCR1A_nominal = calc_timer(current_block->nominal_rate); + OCR1A_nominal = calc_timer(uint16_t(current_block->nominal_rate)); // make a note of the number of step loops required at nominal speed step_loops_nominal = step_loops; - acc_step_rate = current_block->initial_rate; + acc_step_rate = uint16_t(current_block->initial_rate); acceleration_time = calc_timer(acc_step_rate); _NEXT_ISR(acceleration_time); @@ -374,7 +372,6 @@ FORCE_INLINE void trapezoid_generator_reset() { final_estep_rate = (current_block->nominal_rate * current_block->abs_adv_steps_multiplier8) >> 17; } #endif - } // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse. @@ -391,157 +388,144 @@ ISR(TIMER1_COMPA_vect) { #endif } -void isr() { - //if (UVLO) uvlo(); - // If there is no current block, attempt to pop one from the buffer - if (current_block == NULL) { - // Anything in the buffer? - current_block = plan_get_current_block(); - if (current_block != NULL) { +FORCE_INLINE void stepper_next_block() +{ + // Anything in the buffer? + current_block = plan_get_current_block(); + if (current_block != NULL) { #ifdef PAT9125 - fsensor_counter = 0; - fsensor_st_block_begin(current_block); + fsensor_counter = 0; + fsensor_st_block_begin(current_block); #endif //PAT9125 - // The busy flag is set by the plan_get_current_block() call. - // current_block->busy = true; - trapezoid_generator_reset(); + // The busy flag is set by the plan_get_current_block() call. + // current_block->busy = true; + trapezoid_generator_reset(); + if (current_block->flag & BLOCK_FLAG_DDA_LOWRES) { + counter_x.lo = -(current_block->step_event_count.lo >> 1); + counter_y.lo = counter_x.lo; + counter_z.lo = counter_x.lo; + counter_e.lo = counter_x.lo; + } else { counter_x.wide = -(current_block->step_event_count.wide >> 1); counter_y.wide = counter_x.wide; counter_z.wide = counter_x.wide; counter_e.wide = counter_x.wide; - step_events_completed.wide = 0; - - #ifdef Z_LATE_ENABLE - if(current_block->steps_z.wide > 0) { - enable_z(); - _NEXT_ISR(2000); //1ms wait - return; - } - #endif } - else { - _NEXT_ISR(2000); // 1kHz. - } - } - - LastStepMask = 0; - - if (current_block != NULL) { - // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt + step_events_completed.wide = 0; + // Set directions. out_bits = current_block->direction_bits; - - // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY) if((out_bits & (1< -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMINLIMIT) - - #ifdef TMC2130_SG_HOMING - // Stall guard homing turned on - x_min_endstop = (READ(X_TMC2130_DIAG) != 0); - #else - // Normal homing - x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING); - #endif - if(x_min_endstop && old_x_min_endstop && (current_block->steps_x.wide > 0)) { - endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; - endstop_x_hit=true; - step_events_completed.wide = current_block->step_event_count.wide; - } - old_x_min_endstop = x_min_endstop; - #endif - } - } + if ((out_bits & (1< -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMAXLIMIT) - - #ifdef TMC2130_SG_HOMING - // Stall guard homing turned on - x_max_endstop = (READ(X_TMC2130_DIAG) != 0); - #else - // Normal homing - x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING); - #endif - if(x_max_endstop && old_x_max_endstop && (current_block->steps_x.wide > 0)){ - endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; - endstop_x_hit=true; - step_events_completed.wide = current_block->step_event_count.wide; - } - old_x_max_endstop = x_max_endstop; - #endif - } - } +#ifndef LIN_ADVANCE + if ((out_bits & (1 << E_AXIS)) != 0) { // -direction + WRITE(E0_DIR_PIN, + #ifdef SNMM + (snmm_extruder == 0 || snmm_extruder == 2) ? !INVERT_E0_DIR : + #endif // SNMM + INVERT_E0_DIR); + count_direction[E_AXIS] = -1; + } else { // +direction + WRITE(E0_DIR_PIN, + #ifdef SNMM + (snmm_extruder == 0 || snmm_extruder == 2) ? INVERT_E0_DIR : + #endif // SNMM + !INVERT_E0_DIR); + count_direction[E_AXIS] = 1; } +#endif /* LIN_ADVANCE */ + } + else { + _NEXT_ISR(2000); // 1kHz. + } +} +// Check limit switches. +FORCE_INLINE void stepper_check_endstops() +{ + if(check_endstops) + { #ifndef COREXY - if ((out_bits & (1< -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMINLIMIT) - + { + #if ( (defined(X_MIN_PIN) && (X_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMINLIMIT) + #ifdef TMC2130_SG_HOMING + // Stall guard homing turned on + x_min_endstop = (READ(X_TMC2130_DIAG) != 0); + #else + // Normal homing + x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING); + #endif + if(x_min_endstop && old_x_min_endstop && (current_block->steps_x.wide > 0)) { + endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; + endstop_x_hit=true; + step_events_completed.wide = current_block->step_event_count.wide; + } + old_x_min_endstop = x_min_endstop; + #endif + } else { // +direction + #if ( (defined(X_MAX_PIN) && (X_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMAXLIMIT) #ifdef TMC2130_SG_HOMING // Stall guard homing turned on - y_min_endstop = (READ(Y_TMC2130_DIAG) != 0); + x_max_endstop = (READ(X_TMC2130_DIAG) != 0); #else // Normal homing - y_min_endstop = (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING); + x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING); #endif - if(y_min_endstop && old_y_min_endstop && (current_block->steps_y.wide > 0)) { - endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; - endstop_y_hit=true; - step_events_completed.wide = current_block->step_event_count.wide; - } - old_y_min_endstop = y_min_endstop; - #endif - } + if(x_max_endstop && old_x_max_endstop && (current_block->steps_x.wide > 0)){ + endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; + endstop_x_hit=true; + step_events_completed.wide = current_block->step_event_count.wide; + } + old_x_max_endstop = x_max_endstop; + #endif } - else { // +direction - CHECK_ENDSTOPS - { - #if ( (defined(Y_MAX_PIN) && (Y_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMAXLIMIT) - + + #ifndef COREXY + if ((out_bits & (1< -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMINLIMIT) + #ifdef TMC2130_SG_HOMING + // Stall guard homing turned on + y_min_endstop = (READ(Y_TMC2130_DIAG) != 0); + #else + // Normal homing + y_min_endstop = (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING); + #endif + if(y_min_endstop && old_y_min_endstop && (current_block->steps_y.wide > 0)) { + endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; + endstop_y_hit=true; + step_events_completed.wide = current_block->step_event_count.wide; + } + old_y_min_endstop = y_min_endstop; + #endif + } else { // +direction + #if ( (defined(Y_MAX_PIN) && (Y_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMAXLIMIT) #ifdef TMC2130_SG_HOMING // Stall guard homing turned on y_max_endstop = (READ(Y_TMC2130_DIAG) != 0); @@ -549,195 +533,226 @@ void isr() { // Normal homing y_max_endstop = (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING); #endif - if(y_max_endstop && old_y_max_endstop && (current_block->steps_y.wide > 0)){ - endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; - endstop_y_hit=true; - step_events_completed.wide = current_block->step_event_count.wide; - } - old_y_max_endstop = y_max_endstop; - #endif - } - } - - if ((out_bits & (1<steps_y.wide > 0)){ + endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; + endstop_y_hit=true; + step_events_completed.wide = current_block->step_event_count.wide; + } + old_y_max_endstop = y_max_endstop; #endif - - count_direction[Z_AXIS]=-1; - if(check_endstops && ! check_z_endstop) - { - #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT) - #ifdef TMC2130_SG_HOMING - // Stall guard homing turned on - z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0); - #else - z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING); - #endif //TMC2130_SG_HOMING - if(z_min_endstop && old_z_min_endstop && (current_block->steps_z.wide > 0)) { - endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; - endstop_z_hit=true; - step_events_completed.wide = current_block->step_event_count.wide; - } - old_z_min_endstop = z_min_endstop; - #endif - } - } - else { // +direction - WRITE_NC(Z_DIR_PIN,!INVERT_Z_DIR); - - #ifdef Z_DUAL_STEPPER_DRIVERS - WRITE_NC(Z2_DIR_PIN,!INVERT_Z_DIR); - #endif - - count_direction[Z_AXIS]=1; - CHECK_ENDSTOPS - { - #if defined(Z_MAX_PIN) && (Z_MAX_PIN > -1) && !defined(DEBUG_DISABLE_ZMAXLIMIT) - #ifdef TMC2130_SG_HOMING - // Stall guard homing turned on - z_max_endstop = (READ(Z_TMC2130_DIAG) != 0); - #else - z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING); - #endif //TMC2130_SG_HOMING - if(z_max_endstop && old_z_max_endstop && (current_block->steps_z.wide > 0)) { - endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; - endstop_z_hit=true; - step_events_completed.wide = current_block->step_event_count.wide; - } - old_z_max_endstop = z_max_endstop; - #endif - } } - // Supporting stopping on a trigger of the Z-stop induction sensor, not only for the Z-minus movements. - #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT) - if(check_z_endstop) { - // Check the Z min end-stop no matter what. - // Good for searching for the center of an induction target. - #ifdef TMC2130_SG_HOMING - // Stall guard homing turned on - z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0); - #else - z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING); - #endif //TMC2130_SG_HOMING - if(z_min_endstop && old_z_min_endstop) { + if ((out_bits & (1< -1) && !defined(DEBUG_DISABLE_ZMINLIMIT) + if (check_z_endstop) { + #ifdef TMC2130_SG_HOMING + // Stall guard homing turned on + z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0); + #else + z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING); + #endif //TMC2130_SG_HOMING + if(z_min_endstop && old_z_min_endstop && (current_block->steps_z.wide > 0)) { endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; endstop_z_hit=true; step_events_completed.wide = current_block->step_event_count.wide; } old_z_min_endstop = z_min_endstop; - } - #endif - - if ((out_bits & (1 << E_AXIS)) != 0) - { // -direction - //AKU - WRITE(E0_DIR_PIN, -#ifdef SNMM - (snmm_extruder == 0 || snmm_extruder == 2) ? !INVERT_E0_DIR : -#endif // SNMM - INVERT_E0_DIR); - count_direction[E_AXIS] = -1; - } - else - { // +direction - WRITE(E0_DIR_PIN, -#ifdef SNMM - (snmm_extruder == 0 || snmm_extruder == 2) ? INVERT_E0_DIR : -#endif // SNMM - !INVERT_E0_DIR); - count_direction[E_AXIS] = 1; - } - - for(uint8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves) - #ifndef AT90USB - MSerial.checkRx(); // Check for serial chars. - #endif //RP - returned, because missing characters - -#ifdef LIN_ADVANCE - counter_e.wide += current_block->steps_e.wide; - if (counter_e.wide > 0) { - counter_e.wide -= current_block->step_event_count.wide; - count_position[E_AXIS] += count_direction[E_AXIS]; - ((out_bits&(1<steps_x.wide; - if (counter_x.wide > 0) { - WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN); - LastStepMask |= X_AXIS_MASK; -#ifdef DEBUG_XSTEP_DUP_PIN - WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN); -#endif //DEBUG_XSTEP_DUP_PIN - counter_x.wide -= current_block->step_event_count.wide; - count_position[X_AXIS]+=count_direction[X_AXIS]; - WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN); -#ifdef DEBUG_XSTEP_DUP_PIN - WRITE_NC(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN); -#endif //DEBUG_XSTEP_DUP_PIN - } - - counter_y.wide += current_block->steps_y.wide; - if (counter_y.wide > 0) { - WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN); - LastStepMask |= Y_AXIS_MASK; -#ifdef DEBUG_YSTEP_DUP_PIN - WRITE_NC(DEBUG_YSTEP_DUP_PIN,!INVERT_Y_STEP_PIN); -#endif //DEBUG_YSTEP_DUP_PIN - - #ifdef Y_DUAL_STEPPER_DRIVERS - WRITE_NC(Y2_STEP_PIN, !INVERT_Y_STEP_PIN); - #endif - - counter_y.wide -= current_block->step_event_count.wide; - count_position[Y_AXIS]+=count_direction[Y_AXIS]; - WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN); -#ifdef DEBUG_YSTEP_DUP_PIN - WRITE_NC(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN); -#endif //DEBUG_YSTEP_DUP_PIN - - #ifdef Y_DUAL_STEPPER_DRIVERS - WRITE_NC(Y2_STEP_PIN, INVERT_Y_STEP_PIN); - #endif - } - - counter_z.wide += current_block->steps_z.wide; - if (counter_z.wide > 0) { - WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN); - LastStepMask |= Z_AXIS_MASK; - #ifdef Z_DUAL_STEPPER_DRIVERS - WRITE_NC(Z2_STEP_PIN, !INVERT_Z_STEP_PIN); - #endif - - counter_z.wide -= current_block->step_event_count.wide; - count_position[Z_AXIS]+=count_direction[Z_AXIS]; - WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN); - - #ifdef Z_DUAL_STEPPER_DRIVERS - WRITE_NC(Z2_STEP_PIN, INVERT_Z_STEP_PIN); - #endif } - -#ifndef LIN_ADVANCE - counter_e.wide += current_block->steps_e.wide; - if (counter_e.wide > 0) { - WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); - counter_e.wide -= current_block->step_event_count.wide; - count_position[E_AXIS]+=count_direction[E_AXIS]; - WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); -#ifdef PAT9125 - fsensor_counter++; -#endif //PAT9125 + #endif + } else { // +direction + #if defined(Z_MAX_PIN) && (Z_MAX_PIN > -1) && !defined(DEBUG_DISABLE_ZMAXLIMIT) + #ifdef TMC2130_SG_HOMING + // Stall guard homing turned on + z_max_endstop = (READ(Z_TMC2130_DIAG) != 0); + #else + z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING); + #endif //TMC2130_SG_HOMING + if(z_max_endstop && old_z_max_endstop && (current_block->steps_z.wide > 0)) { + endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; + endstop_z_hit=true; + step_events_completed.wide = current_block->step_event_count.wide; } -#endif - - ++ step_events_completed.wide; - if(step_events_completed.wide >= current_block->step_event_count.wide) break; + old_z_max_endstop = z_max_endstop; + #endif } + } + + // Supporting stopping on a trigger of the Z-stop induction sensor, not only for the Z-minus movements. + #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT) + if (check_z_endstop) { + // Check the Z min end-stop no matter what. + // Good for searching for the center of an induction target. + #ifdef TMC2130_SG_HOMING + // Stall guard homing turned on + z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0); + #else + z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING); + #endif //TMC2130_SG_HOMING + if(z_min_endstop && old_z_min_endstop) { + endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; + endstop_z_hit=true; + step_events_completed.wide = current_block->step_event_count.wide; + } + old_z_min_endstop = z_min_endstop; + } + #endif +} + +FORCE_INLINE void stepper_tick_lowres() +{ + for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves) + MSerial.checkRx(); // Check for serial chars. +#ifdef LIN_ADVANCE + counter_e.lo += current_block->steps_e.lo; + if (counter_e.lo > 0) { + counter_e.lo -= current_block->step_event_count.lo; + count_position[E_AXIS] += count_direction[E_AXIS]; + ((out_bits&(1<steps_x.lo; + if (counter_x.lo > 0) { + WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN); + LastStepMask |= X_AXIS_MASK; +#ifdef DEBUG_XSTEP_DUP_PIN + WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN); +#endif //DEBUG_XSTEP_DUP_PIN + counter_x.lo -= current_block->step_event_count.lo; + count_position[X_AXIS]+=count_direction[X_AXIS]; + WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN); +#ifdef DEBUG_XSTEP_DUP_PIN + WRITE_NC(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN); +#endif //DEBUG_XSTEP_DUP_PIN + } + // Step in Y axis + counter_y.lo += current_block->steps_y.lo; + if (counter_y.lo > 0) { + WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN); + LastStepMask |= Y_AXIS_MASK; +#ifdef DEBUG_YSTEP_DUP_PIN + WRITE_NC(DEBUG_YSTEP_DUP_PIN,!INVERT_Y_STEP_PIN); +#endif //DEBUG_YSTEP_DUP_PIN + counter_y.lo -= current_block->step_event_count.lo; + count_position[Y_AXIS]+=count_direction[Y_AXIS]; + WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN); +#ifdef DEBUG_YSTEP_DUP_PIN + WRITE_NC(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN); +#endif //DEBUG_YSTEP_DUP_PIN + } + // Step in Z axis + counter_z.lo += current_block->steps_z.lo; + if (counter_z.lo > 0) { + WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN); + LastStepMask |= Z_AXIS_MASK; + counter_z.lo -= current_block->step_event_count.lo; + count_position[Z_AXIS]+=count_direction[Z_AXIS]; + WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN); + } +#ifndef LIN_ADVANCE + // Step in E axis + counter_e.lo += current_block->steps_e.lo; + if (counter_e.lo > 0) { + WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); + counter_e.lo -= current_block->step_event_count.lo; + count_position[E_AXIS]+=count_direction[E_AXIS]; + WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); +#ifdef PAT9125 + ++ fsensor_counter; +#endif //PAT9125 + } +#endif + if(++ step_events_completed.lo >= current_block->step_event_count.lo) + break; + } +} + +FORCE_INLINE void stepper_tick_highres() +{ + for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves) + MSerial.checkRx(); // Check for serial chars. +#ifdef LIN_ADVANCE + counter_e.wide += current_block->steps_e.wide; + if (counter_e.wide > 0) { + counter_e.wide -= current_block->step_event_count.wide; + count_position[E_AXIS] += count_direction[E_AXIS]; + ((out_bits&(1<steps_x.wide; + if (counter_x.wide > 0) { + WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN); + LastStepMask |= X_AXIS_MASK; +#ifdef DEBUG_XSTEP_DUP_PIN + WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN); +#endif //DEBUG_XSTEP_DUP_PIN + counter_x.wide -= current_block->step_event_count.wide; + count_position[X_AXIS]+=count_direction[X_AXIS]; + WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN); +#ifdef DEBUG_XSTEP_DUP_PIN + WRITE_NC(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN); +#endif //DEBUG_XSTEP_DUP_PIN + } + // Step in Y axis + counter_y.wide += current_block->steps_y.wide; + if (counter_y.wide > 0) { + WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN); + LastStepMask |= Y_AXIS_MASK; +#ifdef DEBUG_YSTEP_DUP_PIN + WRITE_NC(DEBUG_YSTEP_DUP_PIN,!INVERT_Y_STEP_PIN); +#endif //DEBUG_YSTEP_DUP_PIN + counter_y.wide -= current_block->step_event_count.wide; + count_position[Y_AXIS]+=count_direction[Y_AXIS]; + WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN); +#ifdef DEBUG_YSTEP_DUP_PIN + WRITE_NC(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN); +#endif //DEBUG_YSTEP_DUP_PIN + } + // Step in Z axis + counter_z.wide += current_block->steps_z.wide; + if (counter_z.wide > 0) { + WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN); + LastStepMask |= Z_AXIS_MASK; + counter_z.wide -= current_block->step_event_count.wide; + count_position[Z_AXIS]+=count_direction[Z_AXIS]; + WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN); + } +#ifndef LIN_ADVANCE + // Step in E axis + counter_e.wide += current_block->steps_e.wide; + if (counter_e.wide > 0) { + WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); + counter_e.wide -= current_block->step_event_count.wide; + count_position[E_AXIS]+=count_direction[E_AXIS]; + WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); +#ifdef PAT9125 + ++ fsensor_counter; +#endif //PAT9125 + } +#endif + if(++ step_events_completed.wide >= current_block->step_event_count.wide) + break; + } +} + +void isr() { + //if (UVLO) uvlo(); + // If there is no current block, attempt to pop one from the buffer + if (current_block == NULL) + stepper_next_block(); + + LastStepMask = 0; + + if (current_block != NULL) + { + stepper_check_endstops(); + if (current_block->flag & BLOCK_FLAG_DDA_LOWRES) + stepper_tick_lowres(); + else + stepper_tick_highres(); + #ifdef LIN_ADVANCE if (current_block->use_advance_lead) { const int delta_adv_steps = current_estep_rate - current_adv_steps; @@ -754,10 +769,10 @@ void isr() { if (step_events_completed.wide <= (unsigned long int)current_block->accelerate_until) { // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); - acc_step_rate += current_block->initial_rate; + acc_step_rate += uint16_t(current_block->initial_rate); // upper limit - if(acc_step_rate > current_block->nominal_rate) + if(acc_step_rate > uint16_t(current_block->nominal_rate)) acc_step_rate = current_block->nominal_rate; // step_rate to timer interval @@ -776,15 +791,15 @@ void isr() { MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); if(step_rate > acc_step_rate) { // Check step_rate stays positive - step_rate = current_block->final_rate; + step_rate = uint16_t(current_block->final_rate); } else { step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point. } // lower limit - if(step_rate < current_block->final_rate) - step_rate = current_block->final_rate; + if(step_rate < uint16_t(current_block->final_rate)) + step_rate = uint16_t(current_block->final_rate); // step_rate to timer interval timer = calc_timer(step_rate); @@ -830,9 +845,11 @@ void isr() { } #endif //PAT9125 } + #ifdef TMC2130 tmc2130_st_isr(LastStepMask); #endif //TMC2130 + #ifdef DEBUG_STEPPER_TIMER_MISSED // Verify whether the next planned timer interrupt has not been missed already. // This debugging test takes < 1.125us