3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Split the stepper ISR routine into multiple inline functions,

Browse source 
added an optimized DDA routine for moves with less than 32767 ticks.
This commit is contained in:
bubnikv 2018-01-14 22:37:07 +01:00
parent 30b06488ca
commit 7a972fd9b0
3 changed files with 331 additions and 313 deletions

5
Firmware/planner.cpp
View file

@ -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_x.wide != 0) enable_x();
if(block->steps_y.wide != 0) enable_y(); if(block->steps_y.wide != 0) enable_y();
#endif #endif
#ifndef Z_LATE_ENABLE
if(block->steps_z.wide != 0) enable_z(); if(block->steps_z.wide != 0) enable_z();
#endif
// Enable extruder(s) // Enable extruder(s)
if(block->steps_e.wide != 0) 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; block->speed_factor = block->nominal_rate / block->nominal_speed;
calculate_trapezoid_for_block(block, block->entry_speed, safe_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. // Move the buffer head. From now the block may be picked up by the stepper interrupt controller.
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;

4
Firmware/planner.h
View file

@ -50,7 +50,7 @@ union dda_isteps_t
{ {
int32_t wide; int32_t wide;
struct { struct {
uint16_t lo; int16_t lo;
int16_t hi; int16_t hi;
}; };
}; };
@ -94,7 +94,7 @@ typedef struct {
float acceleration; float acceleration;
// Bit flags defined by the BlockFlag enum. // Bit flags defined by the BlockFlag enum.
bool flag; uint8_t flag;
// Settings for the trapezoid generator (runs inside an interrupt handler). // 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. // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts.

635
Firmware/stepper.cpp
View file

@ -135,8 +135,6 @@ extern bool stepper_timer_overflow_state;
//=============================functions ============================ //=============================functions ============================
//=========================================================================== //===========================================================================
#define CHECK_ENDSTOPS if(check_endstops)
#ifndef _NO_ASM #ifndef _NO_ASM
// intRes = intIn1 * intIn2 >> 16 // 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; unsigned short timer;
if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY; 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() { FORCE_INLINE void trapezoid_generator_reset() {
deceleration_time = 0; deceleration_time = 0;
// step_rate to timer interval // 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 // make a note of the number of step loops required at nominal speed
step_loops_nominal = step_loops; 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); acceleration_time = calc_timer(acc_step_rate);
_NEXT_ISR(acceleration_time); _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; final_estep_rate = (current_block->nominal_rate * current_block->abs_adv_steps_multiplier8) >> 17;
} }
#endif #endif
} }
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse. // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
@ -391,157 +388,144 @@ ISR(TIMER1_COMPA_vect) {
#endif #endif
} }
void isr() { FORCE_INLINE void stepper_next_block()
//if (UVLO) uvlo(); {
// If there is no current block, attempt to pop one from the buffer // Anything in the buffer?
if (current_block == NULL) { current_block = plan_get_current_block();
// Anything in the buffer? if (current_block != NULL) {
current_block = plan_get_current_block();
if (current_block != NULL) {
#ifdef PAT9125 #ifdef PAT9125
fsensor_counter = 0; fsensor_counter = 0;
fsensor_st_block_begin(current_block); fsensor_st_block_begin(current_block);
#endif //PAT9125 #endif //PAT9125
// The busy flag is set by the plan_get_current_block() call. // The busy flag is set by the plan_get_current_block() call.
// current_block->busy = true; // current_block->busy = true;
trapezoid_generator_reset(); 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_x.wide = -(current_block->step_event_count.wide >> 1);
counter_y.wide = counter_x.wide; counter_y.wide = counter_x.wide;
counter_z.wide = counter_x.wide; counter_z.wide = counter_x.wide;
counter_e.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 { step_events_completed.wide = 0;
_NEXT_ISR(2000); // 1kHz. // Set directions.
}
}
LastStepMask = 0;
if (current_block != NULL) {
// Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
out_bits = current_block->direction_bits; out_bits = current_block->direction_bits;
// Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY) // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
if((out_bits & (1<<X_AXIS))!=0){ if((out_bits & (1<<X_AXIS))!=0){
WRITE_NC(X_DIR_PIN, INVERT_X_DIR); WRITE_NC(X_DIR_PIN, INVERT_X_DIR);
count_direction[X_AXIS]=-1; count_direction[X_AXIS]=-1;
} } else {
else{ WRITE_NC(X_DIR_PIN, !INVERT_X_DIR);
WRITE_NC(X_DIR_PIN, !INVERT_X_DIR);
count_direction[X_AXIS]=1; count_direction[X_AXIS]=1;
} }
if((out_bits & (1<<Y_AXIS))!=0){ if((out_bits & (1<<Y_AXIS))!=0){
WRITE_NC(Y_DIR_PIN, INVERT_Y_DIR); WRITE_NC(Y_DIR_PIN, INVERT_Y_DIR);
#ifdef Y_DUAL_STEPPER_DRIVERS
WRITE_NC(Y2_DIR_PIN, !(INVERT_Y_DIR == INVERT_Y2_VS_Y_DIR));
#endif
count_direction[Y_AXIS]=-1; count_direction[Y_AXIS]=-1;
} } else {
else{
WRITE_NC(Y_DIR_PIN, !INVERT_Y_DIR); WRITE_NC(Y_DIR_PIN, !INVERT_Y_DIR);
#ifdef Y_DUAL_STEPPER_DRIVERS
WRITE_NC(Y2_DIR_PIN, (INVERT_Y_DIR == INVERT_Y2_VS_Y_DIR));
#endif
count_direction[Y_AXIS]=1; count_direction[Y_AXIS]=1;
} }
if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
// Set direction en check limit switches WRITE_NC(Z_DIR_PIN,INVERT_Z_DIR);
#ifndef COREXY count_direction[Z_AXIS]=-1;
if ((out_bits & (1<<X_AXIS)) != 0) { // stepping along -X axis } else { // +direction
#else WRITE_NC(Z_DIR_PIN,!INVERT_Z_DIR);
if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) { //-X occurs for -A and -B count_direction[Z_AXIS]=1;
#endif
CHECK_ENDSTOPS
{
{
#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 #ifndef LIN_ADVANCE
CHECK_ENDSTOPS if ((out_bits & (1 << E_AXIS)) != 0) { // -direction
{ WRITE(E0_DIR_PIN,
{ #ifdef SNMM
#if ( (defined(X_MAX_PIN) && (X_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMAXLIMIT) (snmm_extruder == 0 || snmm_extruder == 2) ? !INVERT_E0_DIR :
#endif // SNMM
#ifdef TMC2130_SG_HOMING INVERT_E0_DIR);
// Stall guard homing turned on count_direction[E_AXIS] = -1;
x_max_endstop = (READ(X_TMC2130_DIAG) != 0); } else { // +direction
#else WRITE(E0_DIR_PIN,
// Normal homing #ifdef SNMM
x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING); (snmm_extruder == 0 || snmm_extruder == 2) ? INVERT_E0_DIR :
#endif #endif // SNMM
if(x_max_endstop && old_x_max_endstop && (current_block->steps_x.wide > 0)){ !INVERT_E0_DIR);
endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; count_direction[E_AXIS] = 1;
endstop_x_hit=true;
step_events_completed.wide = current_block->step_event_count.wide;
}
old_x_max_endstop = x_max_endstop;
#endif
}
}
} }
#endif /* LIN_ADVANCE */
}
else {
_NEXT_ISR(2000); // 1kHz.
}
}
// Check limit switches.
FORCE_INLINE void stepper_check_endstops()
{
if(check_endstops)
{
#ifndef COREXY #ifndef COREXY
if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction if ((out_bits & (1<<X_AXIS)) != 0) // stepping along -X axis
#else #else
if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) { // -Y occurs for -A and +B if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) //-X occurs for -A and -B
#endif #endif
CHECK_ENDSTOPS {
{ #if ( (defined(X_MIN_PIN) && (X_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMINLIMIT)
#ifdef TMC2130_SG_HOMING
#if ( (defined(Y_MIN_PIN) && (Y_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMINLIMIT) // 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 #ifdef TMC2130_SG_HOMING
// Stall guard homing turned on // Stall guard homing turned on
y_min_endstop = (READ(Y_TMC2130_DIAG) != 0); x_max_endstop = (READ(X_TMC2130_DIAG) != 0);
#else #else
// Normal homing // 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 #endif
if(y_min_endstop && old_y_min_endstop && (current_block->steps_y.wide > 0)) { if(x_max_endstop && old_x_max_endstop && (current_block->steps_x.wide > 0)){
endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
endstop_y_hit=true; endstop_x_hit=true;
step_events_completed.wide = current_block->step_event_count.wide; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_y_min_endstop = y_min_endstop; old_x_max_endstop = x_max_endstop;
#endif #endif
}
} }
else { // +direction
CHECK_ENDSTOPS #ifndef COREXY
{ if ((out_bits & (1<<Y_AXIS)) != 0) // -direction
#if ( (defined(Y_MAX_PIN) && (Y_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMAXLIMIT) #else
if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) // -Y occurs for -A and +B
#endif
{
#if ( (defined(Y_MIN_PIN) && (Y_MIN_PIN > -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 #ifdef TMC2130_SG_HOMING
// Stall guard homing turned on // Stall guard homing turned on
y_max_endstop = (READ(Y_TMC2130_DIAG) != 0); y_max_endstop = (READ(Y_TMC2130_DIAG) != 0);
@ -549,195 +533,226 @@ void isr() {
// Normal homing // Normal homing
y_max_endstop = (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING); y_max_endstop = (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
#endif #endif
if(y_max_endstop && old_y_max_endstop && (current_block->steps_y.wide > 0)){ if(y_max_endstop && old_y_max_endstop && (current_block->steps_y.wide > 0)){
endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
endstop_y_hit=true; endstop_y_hit=true;
step_events_completed.wide = current_block->step_event_count.wide; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_y_max_endstop = y_max_endstop; old_y_max_endstop = y_max_endstop;
#endif
}
}
if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
WRITE_NC(Z_DIR_PIN,INVERT_Z_DIR);
#ifdef Z_DUAL_STEPPER_DRIVERS
WRITE_NC(Z2_DIR_PIN,INVERT_Z_DIR);
#endif #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 ((out_bits & (1<<Z_AXIS)) != 0) // -direction
#if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT) {
if(check_z_endstop) { #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
// Check the Z min end-stop no matter what. if (check_z_endstop) {
// Good for searching for the center of an induction target. #ifdef TMC2130_SG_HOMING
#ifdef TMC2130_SG_HOMING // Stall guard homing turned on
// Stall guard homing turned on z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0); #else
#else z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING); #endif //TMC2130_SG_HOMING
#endif //TMC2130_SG_HOMING if(z_min_endstop && old_z_min_endstop && (current_block->steps_z.wide > 0)) {
if(z_min_endstop && old_z_min_endstop) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true; endstop_z_hit=true;
step_events_completed.wide = current_block->step_event_count.wide; step_events_completed.wide = current_block->step_event_count.wide;
} }
old_z_min_endstop = z_min_endstop; 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<<E_AXIS))!=0) ? --e_steps : ++e_steps;
}
#endif
counter_x.wide += current_block->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
} }
#endif
#ifndef LIN_ADVANCE } else { // +direction
counter_e.wide += current_block->steps_e.wide; #if defined(Z_MAX_PIN) && (Z_MAX_PIN > -1) && !defined(DEBUG_DISABLE_ZMAXLIMIT)
if (counter_e.wide > 0) { #ifdef TMC2130_SG_HOMING
WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); // Stall guard homing turned on
counter_e.wide -= current_block->step_event_count.wide; z_max_endstop = (READ(Z_TMC2130_DIAG) != 0);
count_position[E_AXIS]+=count_direction[E_AXIS]; #else
WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
#ifdef PAT9125 #endif //TMC2130_SG_HOMING
fsensor_counter++; if(z_max_endstop && old_z_max_endstop && (current_block->steps_z.wide > 0)) {
#endif //PAT9125 endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true;
step_events_completed.wide = current_block->step_event_count.wide;
} }
#endif old_z_max_endstop = z_max_endstop;
#endif
++ step_events_completed.wide;
if(step_events_completed.wide >= current_block->step_event_count.wide) break;
} }
}
// 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<<E_AXIS))!=0) ? --e_steps : ++e_steps;
}
#endif
// Step in X axis
counter_x.lo += current_block->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<<E_AXIS))!=0) ? --e_steps : ++e_steps;
}
#endif
// Step in X axis
counter_x.wide += current_block->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 #ifdef LIN_ADVANCE
if (current_block->use_advance_lead) { if (current_block->use_advance_lead) {
const int delta_adv_steps = current_estep_rate - current_adv_steps; 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) { if (step_events_completed.wide <= (unsigned long int)current_block->accelerate_until) {
// v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate
MultiU24X24toH16(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 // 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; acc_step_rate = current_block->nominal_rate;
// step_rate to timer interval // step_rate to timer interval
@ -776,15 +791,15 @@ void isr() {
MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
if(step_rate > acc_step_rate) { // Check step_rate stays positive 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 { else {
step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point. step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
} }
// lower limit // lower limit
if(step_rate < current_block->final_rate) if(step_rate < uint16_t(current_block->final_rate))
step_rate = current_block->final_rate; step_rate = uint16_t(current_block->final_rate);
// step_rate to timer interval // step_rate to timer interval
timer = calc_timer(step_rate); timer = calc_timer(step_rate);
@ -830,9 +845,11 @@ void isr() {
} }
#endif //PAT9125 #endif //PAT9125
} }
#ifdef TMC2130 #ifdef TMC2130
tmc2130_st_isr(LastStepMask); tmc2130_st_isr(LastStepMask);
#endif //TMC2130 #endif //TMC2130
#ifdef DEBUG_STEPPER_TIMER_MISSED #ifdef DEBUG_STEPPER_TIMER_MISSED
// Verify whether the next planned timer interrupt has not been missed already. // Verify whether the next planned timer interrupt has not been missed already.
// This debugging test takes < 1.125us // This debugging test takes < 1.125us