Implemented fast-then-slow G30 probing

2 files changed, 26 insertions, 11 deletions

diff --git a/src/Endstops/ZProbe.cpp b/src/Endstops/ZProbe.cpp
index 4141d2a3..e1dfbcb6 100644
--- a/src/Endstops/ZProbe.cpp
+++ b/src/Endstops/ZProbe.cpp
@@ -51,6 +51,14 @@ constexpr ObjectModelArrayDescriptor ZProbe::temperatureCoefficientsArrayDescrip
 				{ return ExpressionValue(((const ZProbe*)self)->temperatureCoefficients[context.GetLastIndex()], 5); }
 };
 
+constexpr ObjectModelArrayDescriptor ZProbe::speedsArrayDescriptor =
+{
+	nullptr,
+	[] (const ObjectModel *self, const ObjectExplorationContext&) noexcept -> size_t { return ARRAY_SIZE(ZProbe::probeSpeeds); },
+	[] (const ObjectModel *self, ObjectExplorationContext& context) noexcept -> ExpressionValue
+				{ return ExpressionValue(((const ZProbe*)self)->probeSpeeds[context.GetLastIndex()], 1); }
+};
+
 constexpr ObjectModelTableEntry ZProbe::objectModelTable[] =
 {
 	// Within each group, these entries must be in alphabetical order
@@ -63,7 +71,8 @@ constexpr ObjectModelTableEntry ZProbe::objectModelTable[] =
 	{ "maxProbeCount",				OBJECT_MODEL_FUNC((int32_t)self->misc.parts.maxTaps), 				ObjectModelEntryFlags::none },
 	{ "offsets",					OBJECT_MODEL_FUNC_NOSELF(&offsetsArrayDescriptor), 					ObjectModelEntryFlags::none },
 	{ "recoveryTime",				OBJECT_MODEL_FUNC(self->recoveryTime, 1), 							ObjectModelEntryFlags::none },
-	{ "speed",						OBJECT_MODEL_FUNC(self->probeSpeed, 1), 							ObjectModelEntryFlags::none },
+	{ "speed",						OBJECT_MODEL_FUNC(self->probeSpeeds[1], 1), 						ObjectModelEntryFlags::obsolete },
+	{ "speeds",						OBJECT_MODEL_FUNC_NOSELF(&speedsArrayDescriptor), 					ObjectModelEntryFlags::none },
 	{ "temperatureCoefficient",		OBJECT_MODEL_FUNC(self->temperatureCoefficients[0], 5), 			ObjectModelEntryFlags::obsolete },
 	{ "temperatureCoefficients",	OBJECT_MODEL_FUNC_NOSELF(&temperatureCoefficientsArrayDescriptor), 	ObjectModelEntryFlags::none },
 	{ "threshold",					OBJECT_MODEL_FUNC((int32_t)self->adcValue), 						ObjectModelEntryFlags::none },
@@ -74,7 +83,7 @@ constexpr ObjectModelTableEntry ZProbe::objectModelTable[] =
 	{ "value",						OBJECT_MODEL_FUNC_NOSELF(&valueArrayDescriptor), 					ObjectModelEntryFlags::live },
 };
 
-constexpr uint8_t ZProbe::objectModelTableDescriptor[] = { 1, 17 };
+constexpr uint8_t ZProbe::objectModelTableDescriptor[] = { 1, 18 };
 
 DEFINE_GET_OBJECT_MODEL_TABLE(ZProbe)
 
@@ -100,7 +109,7 @@ void ZProbe::SetDefaults() noexcept
 		tc = 0.0;
 	}
 	diveHeight = DefaultZDive;
-	probeSpeed = DefaultProbingSpeed;
+	probeSpeeds[0] = probeSpeeds[1] = DefaultProbingSpeed;
 	travelSpeed = DefaultZProbeTravelSpeed;
 	recoveryTime = 0.0;
 	tolerance = DefaultZProbeTolerance;
@@ -330,7 +339,7 @@ GCodeResult ZProbe::HandleG31(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 		reprap.SensorsUpdated();
 		if (gb.MachineState().runningM501)
 		{
-			misc.parts.saveToConfigOverride = true;		// we are loading these parameters from config-override.g, so a subsequent M500 should save them to config-override.g
+			misc.parts.saveToConfigOverride = true;			// we are loading these parameters from config-override.g, so a subsequent M500 should save them to config-override.g
 		}
 	}
 	else
@@ -365,7 +374,11 @@ GCodeResult ZProbe::Configure(GCodeBuffer& gb, const StringRef &reply, bool& see
 	gb.TryGetFValue('H', diveHeight, seen);					// dive height
 	if (gb.Seen('F'))										// feed rate i.e. probing speed
 	{
-		probeSpeed = gb.GetFValue() * SecondsToMinutes;
+		float userProbeSpeeds[2];
+		size_t numSpeeds = 2;
+		gb.GetFloatArray(userProbeSpeeds, numSpeeds, true);
+		probeSpeeds[0] = userProbeSpeeds[0] * SecondsToMinutes;
+		probeSpeeds[1] = userProbeSpeeds[1] * SecondsToMinutes;
 		seen = true;
 	}
 
@@ -398,9 +411,9 @@ GCodeResult ZProbe::Configure(GCodeBuffer& gb, const StringRef &reply, bool& see
 
 	reply.printf("Z Probe %u: type %u", number, (unsigned int)type);
 	const GCodeResult rslt = AppendPinNames(reply);
-	reply.catf(", dive height %.1fmm, probe speed %dmm/min, travel speed %dmm/min, recovery time %.2f sec, heaters %s, max taps %u, max diff %.2f",
+	reply.catf(", dive height %.1fmm, probe speeds %d,%dmm/min, travel speed %dmm/min, recovery time %.2f sec, heaters %s, max taps %u, max diff %.2f",
 					(double)diveHeight,
-					(int)(probeSpeed * MinutesToSeconds), (int)(travelSpeed * MinutesToSeconds),
+					(int)(probeSpeeds[0] * MinutesToSeconds), (int)(probeSpeeds[1] * MinutesToSeconds), (int)(travelSpeed * MinutesToSeconds),
 					(double)recoveryTime,
 					(misc.parts.turnHeatersOff) ? "suspended" : "normal",
 						misc.parts.maxTaps, (double)tolerance);
diff --git a/src/Endstops/ZProbe.h b/src/Endstops/ZProbe.h
index 23fe8116..d1a0df38 100644
--- a/src/Endstops/ZProbe.h
+++ b/src/Endstops/ZProbe.h
@@ -40,7 +40,8 @@ public:
 	float GetActualTriggerHeight() const noexcept;
 	float GetDiveHeight() const noexcept { return diveHeight; }
 	float GetStartingHeight() const noexcept { return diveHeight + GetActualTriggerHeight(); }
-	float GetProbingSpeed() const noexcept { return probeSpeed; }
+	float GetProbingSpeed(int tapsDone) const noexcept { return probeSpeeds[(tapsDone < 0) ? 0 : 1]; }
+	float HasTwoProbingSpeeds() const noexcept { return probeSpeeds[1] != probeSpeeds[0]; }
 	float GetTravelSpeed() const noexcept { return travelSpeed; }
 	float GetRecoveryTime() const noexcept { return recoveryTime; }
 	float GetTolerance() const noexcept { return tolerance; }
@@ -71,11 +72,12 @@ protected:
 	OBJECT_MODEL_ARRAY(offsets)
 	OBJECT_MODEL_ARRAY(value)
 	OBJECT_MODEL_ARRAY(temperatureCoefficients)
+	OBJECT_MODEL_ARRAY(speeds)
 
 	uint8_t number;
 	ZProbeType type;
-	int8_t sensor;					// the sensor number used for temperature calibration
-	int16_t adcValue;				// the target ADC value, after inversion if enabled
+	int8_t sensor;						// the sensor number used for temperature calibration
+	int16_t adcValue;					// the target ADC value, after inversion if enabled
 	union
 	{
 		struct
@@ -92,7 +94,7 @@ protected:
 	float calibTemperature;				// the temperature at which we did the calibration
 	float temperatureCoefficients[2];	// the variation of height with bed temperature and with the square of temperature
 	float diveHeight;					// the dive height we use when probing
-	float probeSpeed;					// the initial speed of probing
+	float probeSpeeds[2];				// the initial speed of probing
 	float travelSpeed;					// the speed at which we travel to the probe point
 	float recoveryTime;					// Z probe recovery time
 	float tolerance;					// maximum difference between probe heights when doing >1 taps