From b275bfcc32e87739b0f5e6caa47ce63386382fc5 Mon Sep 17 00:00:00 2001
From: Markus Hitter <mah@jump-ing.de>
Date: Sun, 25 May 2014 00:23:05 +0200
Subject: [PATCH] Implement adaptive homing feedrates.

For now for X min only, but it works excellently already.
Tested quite a few combinations and raising acceleration
or endstop clearance raises homing feedrate just as expected.

Quite a chunk of the code is for testing the given configuration,
only. A thing which would ideally be done for every macro
used in each code file.
---
 config.default.h | 30 +++++++++++++++++++++++-----
 home.c           | 52 +++++++++++++++++++++++++++++++++++++++---------
 2 files changed, 68 insertions(+), 14 deletions(-)

diff --git a/config.default.h b/config.default.h
index 36de95a..dbd1db3 100644
--- a/config.default.h
+++ b/config.default.h
@@ -82,17 +82,37 @@
 #define	MAXIMUM_FEEDRATE_Z		100
 #define	MAXIMUM_FEEDRATE_E		200
 
-/// used when searching endstops and as default feedrate
+/// Used when doing precision endstop search and as default feedrate.
 #define	SEARCH_FEEDRATE_X			50
 #define	SEARCH_FEEDRATE_Y			50
 #define	SEARCH_FEEDRATE_Z			50
 // no SEARCH_FEEDRATE_E, as E can't be searched
 
-/** \def SLOW_HOMING
-	wether to search the home point slowly
-		With some endstop configurations, like when probing for the surface of a PCB, you can't deal with overrunning the endstop. In such a case, uncomment this definition.
+/** \def ENDSTOP_CLEARANCE_X
+    \def ENDSTOP_CLEARANCE_Y
+    \def ENDSTOP_CLEARANCE_Z
+
+  When hitting an endstop, Teacup properly decelerates instead of doing an
+  aprupt stop to save your mechanics. Ineviteably, this means it overshoots
+  the endstop trigger point by some distance.
+
+  To deal with this, Teacup adapts homing movement speeds to what your
+  endstops can deal with. The higher the allowed acceleration ( = deceleration,
+  see #define ACCELERATION) and the more clearance the endstop comes with,
+  the faster Teacup will do homing movements.
+
+  Set here how many micrometers (mm * 1000) your endstop allows the carriage
+  to overshoot the trigger point. Typically 1000 or 2000 for mechanical
+  endstops, more for optical ones. You can set it to zero, in which case
+  SEARCH_FEEDRATE_{XYZ} is used, but expect very slow homing movements.
+
+  Units: micrometers
+  Sane values: 0 to 20000   (0 to 20 mm)
+  Valid range: 0 to 1000000
 */
-// #define	SLOW_HOMING
+#define ENDSTOP_CLEARANCE_X 1000
+#define ENDSTOP_CLEARANCE_Y 1000
+#define ENDSTOP_CLEARANCE_Z 100
 
 /**
 	Soft axis limits, in mm.
diff --git a/home.c b/home.c
index 75263cd..894722b 100644
--- a/home.c
+++ b/home.c
@@ -4,11 +4,48 @@
 	\brief Homing routines
 */
 
+#include <math.h>
 #include	"dda.h"
 #include	"dda_queue.h"
 #include	"pinio.h"
 #include	"gcode_parse.h"
 
+// Check configuration.
+#if defined X_MIN_PIN || defined X_MAX_PIN
+  #ifndef SEARCH_FEEDRATE_X
+    #error SEARCH_FEEDRATE_X undefined. It should be defined in config.h.
+  #endif
+  #ifndef ENDSTOP_CLEARANCE_X
+    #error ENDSTOP_CLEARANCE_X undefined. It should be defined in config.h.
+  #endif
+#endif
+#if defined Y_MIN_PIN || defined Y_MAX_PIN
+  #ifndef SEARCH_FEEDRATE_Y
+    #error SEARCH_FEEDRATE_Y undefined. It should be defined in config.h.
+  #endif
+  #ifndef ENDSTOP_CLEARANCE_Y
+    #error ENDSTOP_CLEARANCE_Y undefined. It should be defined in config.h.
+  #endif
+#endif
+#if defined Z_MIN_PIN || defined Z_MAX_PIN
+  #ifndef SEARCH_FEEDRATE_Z
+    #error SEARCH_FEEDRATE_Z undefined. It should be defined in config.h.
+  #endif
+  #ifndef ENDSTOP_CLEARANCE_Z
+    #error ENDSTOP_CLEARANCE_Z undefined. It should be defined in config.h.
+  #endif
+#endif
+
+// Calculate feedrates according to clearance and deceleration.
+// For a description, see #define ENDSTOP_CLEARANCE_{XYZ} in config.h.
+//   s = 1/2 * a * t^2; t = v / a  <==> v = sqrt(2 * a * s))
+//   units: / 1000 for um -> mm; * 60 for mm/s -> mm/min
+#ifdef ENDSTOP_CLEARANCE_X
+  #define SEARCH_FAST_X (uint32_t)((double)60. * \
+            sqrt((double)2 * ACCELERATION * ENDSTOP_CLEARANCE_X / 1000.))
+#endif
+
+
 /// home all 3 axes
 void home() {
 	#if defined	X_MIN_PIN
@@ -36,21 +73,18 @@ void home_x_negative() {
 		TARGET t = startpoint;
 
 		t.X = -1000000;
-		#ifdef SLOW_HOMING
-			// hit home soft
-			t.F = SEARCH_FEEDRATE_X;
-		#else
-			// hit home hard
-			t.F = MAXIMUM_FEEDRATE_X;
-		#endif
+    if (SEARCH_FAST_X > SEARCH_FEEDRATE_X) // Preprocessor can't check this :-/
+      t.F = SEARCH_FAST_X;
+    else
+      t.F = SEARCH_FEEDRATE_X;
 		enqueue_home(&t, 0x1, 1);
 
-		#ifndef SLOW_HOMING
+    if (SEARCH_FAST_X > SEARCH_FEEDRATE_X) {
 			// back off slowly
 			t.X = +1000000;
 			t.F = SEARCH_FEEDRATE_X;
 			enqueue_home(&t, 0x1, 0);
-		#endif
+    }
 
 		// set X home
 		queue_wait(); // we have to wait here, see G92