Fixed thread shift when RPM changes

Fixed thread shift when RPM changes Fixed fluctuations in reported RPM when threading starts
MetalWorkerTools · Oct 11, 2021 · 741951b · 741951b
1 parent b4f925f
commit 741951b
Show file tree

Hide file tree

Showing 13 changed files with 183 additions and 158 deletions.
diff --git a/README.md b/README.md
@@ -1,6 +1,4 @@
-There is a [threading issue](https://github.com/MetalWorkerTools/grbl-L-Mega/issues/5#issue-1016791195). Read the [issue](https://github.com/MetalWorkerTools/grbl-L-Mega/issues/5#issue-1016791195) for more information.
-***
-New features in the current [CNCL](https://www.microsoft.com/store/apps/9P42TB5T697H) release requires some GRBL compiler options changes. These compiler options changes are implemented in this GRBL fork to make it easy to compile and flash the modified GRBL to an Arduino for running on a lathe. The [Compiler Options](https://github.com/HuubBuis/grbl-L-Mega/wiki/Changed-Compiler-options) are described in the [GRBL-L-Mega Wiki](https://github.com/HuubBuis/grbl-L-Mega/wiki). For all other information read the [GRBL Wiki](https://github.com/gnea/grbl/wiki)  
+New features in the current [CNCL](https://www.microsoft.com/store/apps/9P42TB5T697H) release require GRBL changes. These changes are implemented in this GRBL fork. The [Compiler Options](https://github.com/HuubBuis/grbl-L-Mega/wiki/Changed-Compiler-options) are described in the [GRBL-L-Mega Wiki](https://github.com/HuubBuis/grbl-L-Mega/wiki). For all other information read the [GRBL Wiki](https://github.com/gnea/grbl/wiki)  
 I have added spindle sync threading G33 based on [fschill grbl version](https://github.com/fschill/grbl-Mega/tree/spindle_sync) to this GRBL-L-Mega version and the version for the Arduino Uno [GRBL-L](https://github.com/HuubBuis/grbl-L). Read the [threading setup Wiki](https://github.com/HuubBuis/grbl-L-Mega/wiki/Threading-setup-and-use) for an explanation of the threading setup.  
 
-Ramps 1.4 - 1.6 users use the Ramps branch.
+Ramps 1.4 - 1.6 users, use the Ramps branch.
diff --git a/grbl/cpu_map.h b/grbl/cpu_map.h
@@ -38,6 +38,7 @@
   #define Y_STEP_BIT    3 // MEGA2560 Digital Pin 25
   #define Z_STEP_BIT    4 // MEGA2560 Digital Pin 26
   #define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
+  #define STEP_MASK_Z (1<<Z_STEP_BIT) // Z step bit only
 
   // Define step direction output pins. NOTE: All direction pins must be on the same port.
   #define DIRECTION_DDR     DDRC

diff --git a/grbl/gcode.c b/grbl/gcode.c
@@ -1046,52 +1046,53 @@ uint8_t gc_execute_line(char *line)
       if (gc_state.modal.motion == MOTION_MODE_LINEAR) {
         mc_line(gc_block.values.xyz, pl_data);
       } else if (gc_state.modal.motion == MOTION_MODE_SPINDLE_SYNC) {
-		 protocol_buffer_synchronize();			// Sync and finish all remaining buffered motions before moving on.
-		 threading_init(gc_block.values.ijk[Z_AXIS]);		//initialize a threading pass, all counters are cleared, check on index pulses timeout can be done
-		 //pl_data->condition ;		
-		 pl_data->condition |= (PL_COND_FLAG_FEED_PER_REV | PL_COND_FLAG_NO_FEED_OVERRIDE);	//During threading (G33) no feed override. Set condition to allow updating the feed rate at every sync pulse
-		 while (threading_index_pulse_count<SPINDLE_INDEX_PULSES_BEFORE_START_G33){
-			if (get_timer_ticks()>(uint32_t)((uint32_t)1500000*(uint32_t)SPINDLE_INDEX_PULSES_BEFORE_START_G33)){				//Check if the spindle pulses are fast enough
-				 FAIL(STATUS_INDEX_PULSE_TIMEOUT);	
-			}
-			protocol_exec_rt_system();						//process real time commands until the spindle has made enough revolutions or a timeout occurs
-		 }
-		 if (settings.sync_pulses_per_revolution>1) {		//There are synchronization pulses so also waiting for the next synchronization pulse
-		   threading_sync_pulse_count=0;
-		   while (threading_sync_pulse_count==0){
-			 if  (get_timer_ticks()>3000000U)				//Check if the sync pulses are fast enough
-			   FAIL(STATUS_SYNCHRONIZATION_PULSE_TIMEOUT);	
-		   protocol_exec_rt_system();						//process real time commands until the spindle has made enough revolutions or a timeout occurs
-		   }
-		 }
-		 //threading_reset();	//reset to undo counting and processing of the previous index and synchronisation pulses
-		 pl_data->feed_rate=gc_block.values.ijk[Z_AXIS] * threading_index_spindle_speed;		//set the start feed rate 
-         mc_line(gc_block.values.xyz, pl_data);	//execute the motion 
-      } else if (gc_state.modal.motion == MOTION_MODE_SEEK) {
-        pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag.
-        mc_line(gc_block.values.xyz, pl_data);
-      } else if ((gc_state.modal.motion == MOTION_MODE_CW_ARC) || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) {
-        mc_arc(gc_block.values.xyz, pl_data, gc_state.position, gc_block.values.ijk, gc_block.values.r,
-            axis_0, axis_1, axis_linear, bit_istrue(gc_parser_flags,GC_PARSER_ARC_IS_CLOCKWISE));
-      } else {
-        // NOTE: gc_block.values.xyz is returned from mc_probe_cycle with the updated position value. So
-        // upon a successful probing cycle, the machine position and the returned value should be the same.
-        #ifndef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES
-          pl_data->condition |= PL_COND_FLAG_NO_FEED_OVERRIDE;
-        #endif
-        gc_update_pos = mc_probe_cycle(gc_block.values.xyz, pl_data, gc_parser_flags);
-      }  
+        protocol_buffer_synchronize();			            // Sync and finish all remaining buffered motions before moving on.
+        threading_init(gc_block.values.ijk[Z_AXIS]);		// initialize a threading pass, all counters are cleared, check on index pulses timeout can be done
+        pl_data->condition |= (PL_COND_FLAG_FEED_PER_REV | PL_COND_FLAG_NO_FEED_OVERRIDE);	//During threading (G33) no feed override. Set condition to allow updating the feed rate at every sync pulse
+        timekeeper_reset();															// Reset the timekeeper to measure the spindle pulse timeout 
+        while (threading_index_pulse_count<SPINDLE_INDEX_PULSES_BEFORE_START_G33){
+				  if (get_timer_ticks_passed()>(uint32_t)((uint32_t)1500000*(uint32_t)SPINDLE_INDEX_PULSES_BEFORE_START_G33))				//Check if the spindle pulses are fast enough
+					  FAIL(STATUS_INDEX_PULSE_TIMEOUT);	
+				  protocol_exec_rt_system();										// Process real time commands until the spindle has made enough revolutions or a timeout occurs
+			  }
+			  if (settings.sync_pulses_per_revolution>1) {		// There are synchronization pulses so wait for the next synchronization pulse
+				  threading_sync_pulse_count=0;
+				  while (threading_sync_pulse_count==0){
+					  if  (get_timer_ticks_passed()>3000000U)			// Check if the sync pulses are fast enough
+						  FAIL(STATUS_SYNCHRONIZATION_PULSE_TIMEOUT);	
+					  protocol_exec_rt_system();									// Process real time commands until the next sync pulse or a timeout occurs
+				  }
+			  }
+			  protocol_exec_rt_system();											                                      // process real time commands if a sync/index pulse was counted, but not processed yet
+        threading_sync_pulse_count=0;                                                         // This is the start C position for spindle synchronization, even if threading is not started yet. The next line will add the threading distance to this target
+        threading_step_pulse_count=0;                                                         // This is the start Z position 
+				pl_data->feed_rate=gc_block.values.ijk[Z_AXIS] * spindle_rpm;		    // Set the start feed rate 
+        mc_line(gc_block.values.xyz, pl_data);	                                              // Execute the motion 
+				} else if (gc_state.modal.motion == MOTION_MODE_SEEK) {
+					pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag.
+					mc_line(gc_block.values.xyz, pl_data);
+				} else if ((gc_state.modal.motion == MOTION_MODE_CW_ARC) || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) {
+					mc_arc(gc_block.values.xyz, pl_data, gc_state.position, gc_block.values.ijk, gc_block.values.r,
+           axis_0, axis_1, axis_linear, bit_istrue(gc_parser_flags,GC_PARSER_ARC_IS_CLOCKWISE));
+				} else {
+					// NOTE: gc_block.values.xyz is returned from mc_probe_cycle with the updated position value. So
+					// upon a successful probing cycle, the machine position and the returned value should be the same.
+					#ifndef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES
+						pl_data->condition |= PL_COND_FLAG_NO_FEED_OVERRIDE;
+					#endif
+					gc_update_pos = mc_probe_cycle(gc_block.values.xyz, pl_data, gc_parser_flags);
+				}  
 
-      // As far as the parser is concerned, the position is now == target. In reality the
-      // motion control system might still be processing the action and the real tool position
-      // in any intermediate location.
-      if (gc_update_pos == GC_UPDATE_POS_TARGET) {
-        memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); // gc_state.position[] = gc_block.values.xyz[]
-      } else if (gc_update_pos == GC_UPDATE_POS_SYSTEM) {
-        gc_sync_position(); // gc_state.position[] = sys_position
-      } // == GC_UPDATE_POS_NONE
-    }     
-  }
+				// As far as the parser is concerned, the position is now == target. In reality the
+				// motion control system might still be processing the action and the real tool position
+				// in any intermediate location.
+				if (gc_update_pos == GC_UPDATE_POS_TARGET) {
+					memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); // gc_state.position[] = gc_block.values.xyz[]
+				} else if (gc_update_pos == GC_UPDATE_POS_SYSTEM) {
+					gc_sync_position(); // gc_state.position[] = sys_position
+				} // == GC_UPDATE_POS_NONE
+			}     
+		}
 
   // [21. Program flow ]:
   // M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may

diff --git a/grbl/limits.c b/grbl/limits.c
@@ -201,9 +201,9 @@ uint8_t limits_get_state(uint8_t selected_pins) //
   // special pinout for an e-stop, but it is generally recommended to just directly connect
   // your e-stop switch to the Arduino reset pin, since it is the most correct way to do this.
   #ifndef ENABLE_SOFTWARE_DEBOUNCE
-  ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process. 
+    ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process. 
     {
-		system_set_threading_exec_flag(EXEC_SPINDLE_INDEX_PULSE);			// Signal the receive of an index pulse
+      process_spindle_index_pulse();
     }  
   #else
   // Upon limit pin change, Software debounce by enabling watchdog timer that will handle the pin change after a short delay (watchdog time out). 

diff --git a/grbl/main.c b/grbl/main.c
@@ -24,13 +24,13 @@
 
 // Declare system global variable structure
 system_t sys;
-int32_t sys_position[N_AXIS];      // Real-time machine (aka home) position vector in steps.
-int32_t sys_probe_position[N_AXIS]; // Last probe position in machine coordinates and steps.
-volatile uint8_t sys_probe_state;   // Probing state value.  Used to coordinate the probing cycle with stepper ISR.
-volatile uint8_t sys_rt_exec_state;   // Global realtime executor bitflag variable for state management. See EXEC bitmasks.
-volatile uint8_t sys_rt_exec_alarm;   // Global realtime executor bitflag variable for setting various alarms.
-volatile uint8_t sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides.
-volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides.
+int32_t sys_position[N_AXIS];            // Real-time machine (aka home) position vector in steps.
+int32_t sys_probe_position[N_AXIS];      // Last probe position in machine coordinates and steps.
+volatile uint8_t sys_probe_state;                 // Probing state value.  Used to coordinate the probing cycle with stepper ISR.
+volatile uint8_t sys_rt_exec_state;               // Global real time executor bit flag variable for state management. See EXEC bit masks.
+volatile uint8_t sys_rt_exec_alarm;               // Global real time executor bit flag variable for setting various alarms.
+volatile uint8_t sys_rt_exec_motion_override;     // Global real time executor bit flag variable for motion-based overrides.
+volatile uint8_t sys_rt_exec_accessory_override;  // Global real time executor bit flag variable for spindle/coolant overrides.
 
 #ifdef DEBUG
   volatile uint8_t sys_rt_exec_debug;
@@ -89,7 +89,7 @@ int main(void)
     serial_reset_read_buffer(); // Clear serial read buffer
     gc_init(); // Set g-code parser to default state
     spindle_init();
-	timekeeper_init();
+	  timekeeper_init();
     coolant_init();
     limits_init();
     probe_init();

diff --git a/grbl/planner.c b/grbl/planner.c
@@ -261,15 +261,24 @@ float plan_compute_profile_nominal_speed(plan_block_t *block)
 	if (block->condition & PL_COND_FLAG_RAPID_MOTION) { nominal_speed *= (0.01*sys.r_override); }
 	else
 	if (block->condition & PL_COND_FLAG_FEED_PER_REV) { // SPINDLE_SYNC
-		if bit_istrue(threading_exec_flags, EXEC_PLANNER_SYNC_PULSE) {									// there was a synchronization pulse so calculate the feed rate to be at the right position at the next spindle pulse	
-		  system_clear_threading_exec_flag(EXEC_PLANNER_SYNC_PULSE);									// clear the bit to avoid processing again.
-		  threading_millimeters_target-=threading_mm_per_synchronization_pulse;							// calculate the new target					
-		  synchronization_millimeters_error=threading_millimeters_target-block->millimeters;			// calculate the position error. Note that block->millimeters counts down This has to be compensated at the next spindle pulse
-		  block->programmed_rate=(threading_mm_per_index_pulse-synchronization_millimeters_error) / (((float) threading_sync_timer_tics_passed ) / threading_feed_rate_calculation_factor); //calculate the new feed rate to reduce the error.
-	      if (block->programmed_rate>block->rapid_rate)												    // limit speed to max-rate set for this block
-		    block->programmed_rate=block->rapid_rate;													// don't run faster than block->rapid_rate	
-		  }
-		} else {
+		if bit_istrue(threading_exec_flags, EXEC_PLANNER_SYNC_PULSE) {									                 // there was a synchronization pulse so calculate the feed rate to be at the right position at the next spindle pulse	
+		  system_clear_threading_exec_flag(EXEC_PLANNER_SYNC_PULSE);									                   // clear the bit to avoid processing again.
+
+      uint32_t tics_passed=get_timer_ticks()-threading_sync_Last_timer_tics;                         // tics passed since last sync pulse, to calculate the actual spindle position to eliminate processing delays
+      float spindle_feed_passed=(((float) tics_passed) / ((float) threading_sync_timer_tics_passed)) * threading_mm_per_synchronization_pulse; // the feed passed since last sync pulse is processed
+      float spindle_position=threading_millimeters_target-spindle_feed_passed-((float) threading_sync_pulse_count)*threading_mm_per_synchronization_pulse;                        // The actual position of the spindle expressed as z position. Note that position counts down 
+
+      int32_t step_count;
+      ATOMIC_BLOCK(ATOMIC_RESTORESTATE){step_count=threading_step_pulse_count;}                       // Block updating the counter by the stepper interrupt while reading
+      float z_position=threading_millimeters_target-((float)step_count)/settings.steps_per_mm[Z_AXIS];// Calculate the actual position based on the step pulses done and the total feed for this thread pass
+
+		  threading_synchronization_millimeters_error=spindle_position-z_position;	                      // calculate the position error. 
+
+		  block->programmed_rate=(threading_mm_per_index_pulse-threading_synchronization_millimeters_error) / (((float) threading_sync_timer_tics_passed ) / threading_feed_rate_calculation_factor); //calculate the new feed rate to reduce the error.
+	    if (block->programmed_rate>block->rapid_rate)												                             // limit speed to max-rate set for this block
+		    block->programmed_rate=block->rapid_rate;													                          
+		}
+	} else {
 		if (!(block->condition & PL_COND_FLAG_NO_FEED_OVERRIDE)) { nominal_speed *= (0.01*sys.f_override); }
 		if (nominal_speed > block->rapid_rate) { nominal_speed = block->rapid_rate; }
 	}
@@ -397,6 +406,11 @@ uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data)
   block->acceleration = limit_value_by_axis_maximum(settings.acceleration, unit_vec);
   block->rapid_rate = limit_value_by_axis_maximum(settings.max_rate, unit_vec);
 
+  // In feed per revolution mode set the feed distance.
+  if ((block->condition & PL_COND_FLAG_FEED_PER_REV)) {
+    threading_millimeters_target=block->millimeters;             // The feed required
+  }
+
   // Store programmed rate.
   if (block->condition & PL_COND_FLAG_RAPID_MOTION) { block->programmed_rate = block->rapid_rate; }
   else { 
@@ -477,10 +491,6 @@ uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data)
     // Finish up by recalculating the plan with the new block.
     planner_recalculate();
   }
-  //In feed per revolution mode set the global threading mm value. This value is defined global to save memory
-  if ((block->condition & PL_COND_FLAG_FEED_PER_REV)) {
-		threading_millimeters_target=block->millimeters;
-  }
   return(PLAN_OK);
 }