Merge pull request #586 from Paciente8159/modified-probe-control

modified probing for better precision
Paciente8159 · Jan 17, 2024 · 6f89167 · 6f89167
2 parents 6ddd89e + 47278ba
commit 6f89167
Show file tree

Hide file tree

Showing 6 changed files with 41 additions and 17 deletions.
diff --git a/uCNC/src/cnc_hal_config_helper.h b/uCNC/src/cnc_hal_config_helper.h
@@ -195,6 +195,12 @@ extern "C"
 #define PROBE_PULLUP
 #endif
 
+#ifdef ENABLE_RT_PROBE_CHECKING
+#ifdef PROBE_ISR
+#undef PROBE_ISR
+#endif
+#endif
+
 #ifdef ESTOP_PULLUP_ENABLE
 #define ESTOP_PULLUP
 #endif

diff --git a/uCNC/src/core/interpolator.c b/uCNC/src/core/interpolator.c
@@ -897,6 +897,10 @@ MCU_CALLBACK void mcu_step_cb(void)
 		return;
 	}
 
+#ifdef ENABLE_RT_PROBE_CHECKING
+	mcu_probe_changed_cb();
+#endif
+
 	uint8_t new_stepbits = stepbits;
 	uint8_t dirs = 0;
 

diff --git a/uCNC/src/core/io_control.c b/uCNC/src/core/io_control.c
@@ -208,10 +208,15 @@ MCU_IO_CALLBACK void mcu_probe_changed_cb(void)
 
 	io_last_probe = probe;
 
-	// stops the machine
-	itp_stop();
 	// stores rt position
-	parser_sync_probe();
+	__ATOMIC__
+	{
+		parser_sync_probe();
+	}
+
+	// instead of stopping the machine does a controlled stop (hold)
+	// itp_stop();
+	cnc_set_exec_state(EXEC_HOLD);
 #endif
 }
 

diff --git a/uCNC/src/core/motion_control.c b/uCNC/src/core/motion_control.c
@@ -912,34 +912,35 @@ uint8_t mc_probe(float *target, uint8_t flags, motion_data_t *block_data)
 
 	// enable the probe
 	io_enable_probe();
-	mcu_probe_changed_cb();
 	mc_line(target, block_data);
 
+	//similar to itp_sync
 	do
 	{
+		if (!cnc_dotasks())
+		{
+			break;
+		}
 		if (io_get_probe() ^ (flags & 0x01))
 		{
+#ifndef ENABLE_RT_PROBE_CHECKING
 			mcu_probe_changed_cb();
+#endif
 			break;
 		}
-	} while (cnc_dotasks() && cnc_get_exec_state(EXEC_RUN));
+	} while (!itp_is_empty() || !planner_buffer_is_empty());
 
+	// wait for a stop
+	while (cnc_dotasks() && cnc_get_exec_state(EXEC_RUN));
 	// disables the probe
 	io_disable_probe();
-
-	// clears HALT state if possible
-	cnc_unlock(true);
-
 	itp_clear();
 	planner_clear();
-	parser_update_probe_pos();
+	// clears hold
+	cnc_clear_exec_state(EXEC_HOLD);
+
 	// sync the position of the motion control
 	mc_sync_position();
-	// HALT could not be cleared. Something is wrong
-	if (cnc_get_exec_state(EXEC_UNHOMED))
-	{
-		return STATUS_CRITICAL_FAIL;
-	}
 
 	cnc_delay_ms(g_settings.debounce_ms); // adds a delay before reading io pin (debounce)
 	probe_ok = io_get_probe();
@@ -952,7 +953,6 @@ uint8_t mc_probe(float *target, uint8_t flags, motion_data_t *block_data)
 		}
 		return STATUS_OK;
 	}
-
 #endif
 
 	return STATUS_PROBE_SUCCESS;
@@ -1128,7 +1128,7 @@ uint8_t mc_build_hmap(float *target, float *offset, float retract_h, motion_data
 
 			// store position
 			int32_t probe_position[STEPPER_COUNT];
-			itp_get_rt_position(probe_position);
+			parser_get_probe(probe_position);
 			kinematics_steps_to_coordinates(probe_position, position);
 			hmap_offsets[i + H_MAPING_GRID_FACTOR * j] = position[AXIS_TOOL];
 			protocol_send_probe_result(1);

diff --git a/uCNC/src/core/parser.c b/uCNC/src/core/parser.c
@@ -291,6 +291,10 @@ void parser_sync_probe(void)
 	itp_get_rt_position(rt_probe_step_pos);
 }
 
+void parser_get_probe(int32_t *position){
+	memcpy(position, rt_probe_step_pos, sizeof(rt_probe_step_pos));
+}
+
 void parser_update_probe_pos(void)
 {
 	kinematics_steps_to_coordinates(rt_probe_step_pos, parser_parameters.last_probe_position);
@@ -1711,8 +1715,12 @@ uint8_t parser_exec_command(parser_state_t *new_state, parser_words_t *words, pa
 			}
 			else
 			{
+				// failed at this position
+				parser_sync_probe();
 				parser_parameters.last_probe_ok = 0;
 			}
+			// sync probe position
+			parser_update_probe_pos();
 
 			if (error == STATUS_OK)
 			{

diff --git a/uCNC/src/core/parser.h b/uCNC/src/core/parser.h
@@ -301,6 +301,7 @@ extern "C"
 	void parser_get_coordsys(uint8_t system_num, float *axis);
 	bool parser_get_wco(float *axis);
 	void parser_sync_probe(void);
+	void parser_get_probe(int32_t *position);
 	void parser_update_probe_pos(void);
 	uint8_t parser_get_probe_result(void);
 	void parser_parameters_load(void);