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

@@ -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_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
   }
@@ -917,7 +915,7 @@ Having the real displacement of the head, we can calculate the total movement le
   #endif
   delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
   delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0;
-  if ( block->steps_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(

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

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<<E_AXIS))!=0) ? --e_steps : ++e_steps;
         }
 #endif
         
-        counter_x += current_block->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);