From a1fd50ea9aaab1ae3a5d69446cf94ba6cc6117be Mon Sep 17 00:00:00 2001 From: bubnikv Date: Sun, 14 Jan 2018 14:45:27 +0100 Subject: [PATCH 1/4] Simlified the extruder ticking code. --- Firmware/stepper.cpp | 30 ++++++++---------------------- Firmware/stepper.h | 14 -------------- 2 files changed, 8 insertions(+), 36 deletions(-) diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index 38ece7ea..5cf3d908 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -634,34 +634,20 @@ void isr() { if ((out_bits & (1 << E_AXIS)) != 0) { // -direction //AKU + WRITE(E0_DIR_PIN, #ifdef SNMM - if (snmm_extruder == 0 || snmm_extruder == 2) - { - NORM_E_DIR(); - } - else - { - REV_E_DIR(); - } -#else - REV_E_DIR(); + (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 - if (snmm_extruder == 0 || snmm_extruder == 2) - { - REV_E_DIR(); - } - else - { - NORM_E_DIR(); - } -#else - NORM_E_DIR(); + (snmm_extruder == 0 || snmm_extruder == 2) ? INVERT_E0_DIR : #endif // SNMM + !INVERT_E0_DIR); count_direction[E_AXIS] = 1; } @@ -738,10 +724,10 @@ void isr() { #ifndef LIN_ADVANCE counter_e += current_block->steps_e; if (counter_e > 0) { - WRITE_E_STEP(!INVERT_E_STEP_PIN); + WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); counter_e -= current_block->step_event_count; count_position[E_AXIS]+=count_direction[E_AXIS]; - WRITE_E_STEP(INVERT_E_STEP_PIN); + WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); #ifdef PAT9125 fsensor_counter++; #endif //PAT9125 diff --git a/Firmware/stepper.h b/Firmware/stepper.h index 9915de90..5ca68a39 100644 --- a/Firmware/stepper.h +++ b/Firmware/stepper.h @@ -23,20 +23,6 @@ #include "planner.h" -#if EXTRUDERS > 2 - #define WRITE_E_STEP(v) { if(current_block->active_extruder == 2) { WRITE(E2_STEP_PIN, v); } else { if(current_block->active_extruder == 1) { WRITE(E1_STEP_PIN, v); } else { WRITE(E0_STEP_PIN, v); }}} - #define NORM_E_DIR() { if(current_block->active_extruder == 2) { WRITE(E2_DIR_PIN, !INVERT_E2_DIR); } else { if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, !INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, !INVERT_E0_DIR); }}} - #define REV_E_DIR() { if(current_block->active_extruder == 2) { WRITE(E2_DIR_PIN, INVERT_E2_DIR); } else { if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, INVERT_E0_DIR); }}} -#elif EXTRUDERS > 1 - #define WRITE_E_STEP(v) { if(current_block->active_extruder == 1) { WRITE(E1_STEP_PIN, v); } else { WRITE(E0_STEP_PIN, v); }} - #define NORM_E_DIR() { if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, !INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, !INVERT_E0_DIR); }} - #define REV_E_DIR() { if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, INVERT_E0_DIR); }} -#else - #define WRITE_E_STEP(v) WRITE(E0_STEP_PIN, v) - #define NORM_E_DIR() WRITE(E0_DIR_PIN, !INVERT_E0_DIR) - #define REV_E_DIR() WRITE(E0_DIR_PIN, INVERT_E0_DIR) -#endif - #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED extern bool abort_on_endstop_hit; #endif From 30b06488ca2d097366af4a1c7d3a47ac1743a58c Mon Sep 17 00:00:00 2001 From: bubnikv Date: Sun, 14 Jan 2018 17:01:04 +0100 Subject: [PATCH 2/4] Redefined the DDA step and accumulator values to unions to support access to the low / high words of the 32bit values. This is a prerequisity for an optimized 16bit only DDA in case the number of step is lower than 32767. --- Firmware/planner.cpp | 108 +++++++++++++++++++++---------------------- Firmware/planner.h | 26 ++++++++++- Firmware/stepper.cpp | 83 +++++++++++++++++---------------- 3 files changed, 119 insertions(+), 98 deletions(-) diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index a5fa0261..5d0e703e 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -227,8 +227,8 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will // have to use intersection_distance() to calculate when to abort acceleration and start braking // in order to reach the final_rate exactly at the end of this block. - if (accel_decel_steps < block->step_event_count) { - plateau_steps = block->step_event_count - accel_decel_steps; + if (accel_decel_steps < block->step_event_count.wide) { + plateau_steps = block->step_event_count.wide - accel_decel_steps; } else { uint32_t acceleration_x4 = acceleration << 2; // Avoid negative numbers @@ -240,26 +240,26 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit accelerate_steps = (block->step_event_count >> 1) + (final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1 + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; #else accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1; - if (block->step_event_count & 1) + if (block->step_event_count.wide & 1) accelerate_steps += acceleration_x2; accelerate_steps /= acceleration_x4; - accelerate_steps += (block->step_event_count >> 1); + accelerate_steps += (block->step_event_count.wide >> 1); #endif - if (accelerate_steps > block->step_event_count) - accelerate_steps = block->step_event_count; + if (accelerate_steps > block->step_event_count.wide) + accelerate_steps = block->step_event_count.wide; } else { #if 0 decelerate_steps = (block->step_event_count >> 1) + (initial_rate_sqr - final_rate_sqr + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; #else decelerate_steps = initial_rate_sqr - final_rate_sqr; - if (block->step_event_count & 1) + if (block->step_event_count.wide & 1) decelerate_steps += acceleration_x2; decelerate_steps /= acceleration_x4; - decelerate_steps += (block->step_event_count >> 1); + decelerate_steps += (block->step_event_count.wide >> 1); #endif - if (decelerate_steps > block->step_event_count) - decelerate_steps = block->step_event_count; - accelerate_steps = block->step_event_count - decelerate_steps; + if (decelerate_steps > block->step_event_count.wide) + decelerate_steps = block->step_event_count.wide; + accelerate_steps = block->step_event_count.wide - decelerate_steps; } } @@ -449,10 +449,10 @@ void getHighESpeed() uint8_t block_index = block_buffer_tail; while(block_index != block_buffer_head) { - if((block_buffer[block_index].steps_x != 0) || - (block_buffer[block_index].steps_y != 0) || - (block_buffer[block_index].steps_z != 0)) { - float se=(float(block_buffer[block_index].steps_e)/float(block_buffer[block_index].step_event_count))*block_buffer[block_index].nominal_speed; + if((block_buffer[block_index].steps_x.wide != 0) || + (block_buffer[block_index].steps_y.wide != 0) || + (block_buffer[block_index].steps_z.wide != 0)) { + float se=(float(block_buffer[block_index].steps_e.wide)/float(block_buffer[block_index].step_event_count.wide))*block_buffer[block_index].nominal_speed; //se; mm/sec; if(se>high) { @@ -493,10 +493,10 @@ void check_axes_activity() while(block_index != block_buffer_head) { block = &block_buffer[block_index]; - if(block->steps_x != 0) x_active++; - if(block->steps_y != 0) y_active++; - if(block->steps_z != 0) z_active++; - if(block->steps_e != 0) e_active++; + if(block->steps_x.wide != 0) x_active++; + if(block->steps_y.wide != 0) y_active++; + if(block->steps_z.wide != 0) z_active++; + if(block->steps_e.wide != 0) e_active++; block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); } } @@ -769,26 +769,24 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate // Number of steps for each axis #ifndef COREXY // default non-h-bot planning -block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); -block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); +block->steps_x.wide = labs(target[X_AXIS]-position[X_AXIS]); +block->steps_y.wide = labs(target[Y_AXIS]-position[Y_AXIS]); #else // corexy planning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html -block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS])); -block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS])); +block->steps_x.wide = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS])); +block->steps_y.wide = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS])); #endif - block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); - block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); + block->steps_z.wide = labs(target[Z_AXIS]-position[Z_AXIS]); + block->steps_e.wide = labs(target[E_AXIS]-position[E_AXIS]); if (volumetric_multiplier[active_extruder] != 1.f) - block->steps_e *= volumetric_multiplier[active_extruder]; - if (extrudemultiply != 100) { - block->steps_e *= extrudemultiply; - block->steps_e /= 100; - } - block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e))); + block->steps_e.wide *= volumetric_multiplier[active_extruder]; + if (extrudemultiply != 100) + block->steps_e.wide *= extrudemultiply * 0.01; + block->step_event_count.wide = max(block->steps_x.wide, max(block->steps_y.wide, max(block->steps_z.wide, block->steps_e.wide))); // Bail if this is a zero-length block - if (block->step_event_count <= dropsegments) + if (block->step_event_count.wide <= dropsegments) { #ifdef PLANNER_DIAGNOSTICS planner_update_queue_min_counter(); @@ -832,21 +830,21 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi //enable active axes #ifdef COREXY - if((block->steps_x != 0) || (block->steps_y != 0)) + if((block->steps_x.wide != 0) || (block->steps_y.wide != 0)) { enable_x(); enable_y(); } #else - if(block->steps_x != 0) enable_x(); - if(block->steps_y != 0) enable_y(); + 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 != 0) enable_z(); + if(block->steps_z.wide != 0) enable_z(); #endif // Enable extruder(s) - if(block->steps_e != 0) + if(block->steps_e.wide != 0) { if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder { @@ -888,7 +886,7 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi } } - if (block->steps_e == 0) + if (block->steps_e.wide == 0) { if(feed_ratesteps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ) + if ( block->steps_x.wide <=dropsegments && block->steps_y.wide <=dropsegments && block->steps_z.wide <=dropsegments ) { block->millimeters = fabs(delta_mm[E_AXIS]); } @@ -950,7 +948,7 @@ Having the real displacement of the head, we can calculate the total movement le #endif // SLOWDOWN block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 - block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 + block->nominal_rate = ceil(block->step_event_count.wide * inverse_second); // (step/sec) Always > 0 #ifdef FILAMENT_SENSOR //FMM update ring buffer used for delay with filament measurements @@ -1027,8 +1025,8 @@ Having the real displacement of the head, we can calculate the total movement le // Compute and limit the acceleration rate for the trapezoid generator. // block->step_event_count ... event count of the fastest axis // block->millimeters ... Euclidian length of the XYZ movement or the E length, if no XYZ movement. - float steps_per_mm = block->step_event_count/block->millimeters; - if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) + float steps_per_mm = block->step_event_count.wide/block->millimeters; + if(block->steps_x.wide == 0 && block->steps_y.wide == 0 && block->steps_z.wide == 0) { block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } @@ -1038,29 +1036,29 @@ Having the real displacement of the head, we can calculate the total movement le #ifdef TMC2130 if (tmc2130_mode == TMC2130_MODE_SILENT) { - if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > SILENT_MAX_ACCEL_X_ST) + if(((float)block->acceleration_st * (float)block->steps_x.wide / (float)block->step_event_count.wide) > SILENT_MAX_ACCEL_X_ST) block->acceleration_st = SILENT_MAX_ACCEL_X_ST; - if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > SILENT_MAX_ACCEL_Y_ST) + if(((float)block->acceleration_st * (float)block->steps_y.wide / (float)block->step_event_count.wide) > SILENT_MAX_ACCEL_Y_ST) block->acceleration_st = SILENT_MAX_ACCEL_Y_ST; } - if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_x.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[X_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; - if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_y.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[Y_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; - if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_e.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[E_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; - if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_z.wide / (float)block->step_event_count.wide ) > axis_steps_per_sqr_second[Z_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; #else //TMC2130 // Limit acceleration per axis //FIXME Vojtech: One shall rather limit a projection of the acceleration vector instead of using the limit. - if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_x.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[X_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; - if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_y.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[Y_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; - if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_e.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[E_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; - if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_z.wide / (float)block->step_event_count.wide ) > axis_steps_per_sqr_second[Z_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; #endif //TMC2130 } @@ -1218,10 +1216,10 @@ Having the real displacement of the head, we can calculate the total movement le // The math is good, but we must avoid retract moves with advance! // de_float > 0.0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves) // - block->use_advance_lead = block->steps_e - && (block->steps_x || block->steps_y) + block->use_advance_lead = block->steps_e.wide + && (block->steps_x.wide || block->steps_y.wide) && extruder_advance_k - && (uint32_t)block->steps_e != block->step_event_count + && (uint32_t)block->steps_e.wide != block->step_event_count.wide && de_float > 0.0; if (block->use_advance_lead) block->abs_adv_steps_multiplier8 = lround( diff --git a/Firmware/planner.h b/Firmware/planner.h index 3b9e65a1..91c5c5fa 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -40,6 +40,28 @@ enum BlockFlag { // If set, the machine will start from a halt at the start of this block, // respecting the maximum allowed jerk. BLOCK_FLAG_START_FROM_FULL_HALT = 4, + // If set, the stepper interrupt expects, that the number of steps to tick will be lower + // than 32767, therefore the DDA algorithm may run with 16bit resolution only. + // In addition, the stepper routine will not do any end stop checking for higher performance. + BLOCK_FLAG_DDA_LOWRES = 8, +}; + +union dda_isteps_t +{ + int32_t wide; + struct { + uint16_t lo; + int16_t hi; + }; +}; + +union dda_usteps_t +{ + uint32_t wide; + struct { + uint16_t lo; + uint16_t hi; + }; }; // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in @@ -47,8 +69,8 @@ enum BlockFlag { typedef struct { // Fields used by the bresenham algorithm for tracing the line // steps_x.y,z, step_event_count, acceleration_rate, direction_bits and active_extruder are set by plan_buffer_line(). - long steps_x, steps_y, steps_z, steps_e; // Step count along each axis - unsigned long step_event_count; // The number of step events required to complete this block + dda_isteps_t steps_x, steps_y, steps_z, steps_e; // Step count along each axis + dda_usteps_t step_event_count; // The number of step events required to complete this block long acceleration_rate; // The acceleration rate used for acceleration calculation unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) unsigned char active_extruder; // Selects the active extruder diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index 5cf3d908..4b53ee89 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -62,11 +62,12 @@ bool z_max_endstop = false; // Variables used by The Stepper Driver Interrupt static unsigned char out_bits; // The next stepping-bits to be output -static int32_t counter_x, // Counter variables for the bresenham line tracer +static dda_isteps_t + counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z, counter_e; -volatile uint32_t step_events_completed; // The number of step events executed in the current block +volatile dda_usteps_t step_events_completed; // The number of step events executed in the current block static int32_t acceleration_time, deceleration_time; //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate; static uint16_t acc_step_rate; // needed for deccelaration start point @@ -404,14 +405,14 @@ void isr() { // The busy flag is set by the plan_get_current_block() call. // current_block->busy = true; trapezoid_generator_reset(); - counter_x = -(current_block->step_event_count >> 1); - counter_y = counter_x; - counter_z = counter_x; - counter_e = counter_x; - step_events_completed = 0; + 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 > 0) { + if(current_block->steps_z.wide > 0) { enable_z(); _NEXT_ISR(2000); //1ms wait return; @@ -476,10 +477,10 @@ void isr() { // 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 > 0)) { + 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 = current_block->step_event_count; + step_events_completed.wide = current_block->step_event_count.wide; } old_x_min_endstop = x_min_endstop; #endif @@ -499,10 +500,10 @@ void isr() { // 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 > 0)){ + 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 = current_block->step_event_count; + step_events_completed.wide = current_block->step_event_count.wide; } old_x_max_endstop = x_max_endstop; #endif @@ -527,10 +528,10 @@ void isr() { // 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 > 0)) { + 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 = current_block->step_event_count; + step_events_completed.wide = current_block->step_event_count.wide; } old_y_min_endstop = y_min_endstop; #endif @@ -548,10 +549,10 @@ 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 > 0)){ + 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 = current_block->step_event_count; + step_events_completed.wide = current_block->step_event_count.wide; } old_y_max_endstop = y_max_endstop; #endif @@ -575,10 +576,10 @@ void isr() { #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 > 0)) { + 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 = current_block->step_event_count; + step_events_completed.wide = current_block->step_event_count.wide; } old_z_min_endstop = z_min_endstop; #endif @@ -601,10 +602,10 @@ void isr() { #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 > 0)) { + 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 = current_block->step_event_count; + step_events_completed.wide = current_block->step_event_count.wide; } old_z_max_endstop = z_max_endstop; #endif @@ -625,7 +626,7 @@ void isr() { if(z_min_endstop && old_z_min_endstop) { endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; endstop_z_hit=true; - step_events_completed = current_block->step_event_count; + step_events_completed.wide = current_block->step_event_count.wide; } old_z_min_endstop = z_min_endstop; } @@ -657,22 +658,22 @@ void isr() { #endif //RP - returned, because missing characters #ifdef LIN_ADVANCE - counter_e += current_block->steps_e; - if (counter_e > 0) { - counter_e -= current_block->step_event_count; + 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; - if (counter_x > 0) { + 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 -= current_block->step_event_count; + 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 @@ -680,8 +681,8 @@ void isr() { #endif //DEBUG_XSTEP_DUP_PIN } - counter_y += current_block->steps_y; - if (counter_y > 0) { + 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 @@ -692,7 +693,7 @@ void isr() { WRITE_NC(Y2_STEP_PIN, !INVERT_Y_STEP_PIN); #endif - counter_y -= current_block->step_event_count; + 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 @@ -704,15 +705,15 @@ void isr() { #endif } - counter_z += current_block->steps_z; - if (counter_z > 0) { + 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 -= current_block->step_event_count; + 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); @@ -722,10 +723,10 @@ void isr() { } #ifndef LIN_ADVANCE - counter_e += current_block->steps_e; - if (counter_e > 0) { + counter_e.wide += current_block->steps_e.wide; + if (counter_e.wide > 0) { WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); - counter_e -= current_block->step_event_count; + 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 @@ -734,8 +735,8 @@ void isr() { } #endif - step_events_completed += 1; - if(step_events_completed >= current_block->step_event_count) break; + ++ step_events_completed.wide; + if(step_events_completed.wide >= current_block->step_event_count.wide) break; } #ifdef LIN_ADVANCE if (current_block->use_advance_lead) { @@ -750,7 +751,7 @@ void isr() { // Calculare new timer value unsigned short timer; uint16_t step_rate; - if (step_events_completed <= (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 MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); acc_step_rate += current_block->initial_rate; @@ -771,7 +772,7 @@ void isr() { eISR_Rate = ADV_RATE(timer, step_loops); #endif } - else if (step_events_completed > (unsigned long int)current_block->decelerate_after) { + else if (step_events_completed.wide > (unsigned long int)current_block->decelerate_after) { MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); if(step_rate > acc_step_rate) { // Check step_rate stays positive @@ -811,7 +812,7 @@ void isr() { } // If current block is finished, reset pointer - if (step_events_completed >= current_block->step_event_count) { + if (step_events_completed.wide >= current_block->step_event_count.wide) { #ifdef PAT9125 fsensor_st_block_chunk(current_block, fsensor_counter); From 7a972fd9b07f3a6885ba06f5f7cc8f282361b50c Mon Sep 17 00:00:00 2001 From: bubnikv Date: Sun, 14 Jan 2018 22:37:07 +0100 Subject: [PATCH 3/4] 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 5d0e703e..41a4e8ea 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 91c5c5fa..ecac73d1 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 4b53ee89..bb020c44 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 From 1eac2b4ccbfdd353a050670caa519e16ae396a27 Mon Sep 17 00:00:00 2001 From: bubnikv Date: Mon, 15 Jan 2018 12:00:28 +0100 Subject: [PATCH 4/4] Fixed a regression error from the last commit regarding Z homing. Removed unused Z_LATE_ENABLE symbol. --- Firmware/Configuration_adv.h | 2 -- Firmware/stepper.cpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/Firmware/Configuration_adv.h b/Firmware/Configuration_adv.h index 36b247cd..90f72462 100644 --- a/Firmware/Configuration_adv.h +++ b/Firmware/Configuration_adv.h @@ -127,8 +127,6 @@ //END AUTOSET LOCATIONS OF LIMIT SWITCHES -ZP -//#define Z_LATE_ENABLE // Enable Z the last moment. Needed if your Z driver overheats. - // A single Z stepper driver is usually used to drive 2 stepper motors. // Uncomment this define to utilize a separate stepper driver for each Z axis motor. // Only a few motherboards support this, like RAMPS, which have dual extruder support (the 2nd, often unused, extruder driver is used diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index bb020c44..8ea867de 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -545,7 +545,7 @@ FORCE_INLINE void stepper_check_endstops() if ((out_bits & (1< -1) && !defined(DEBUG_DISABLE_ZMINLIMIT) - if (check_z_endstop) { + 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);