Implement G38.x (2 <= x <= 5) (Duet3D#324)

* Implement G38.x (2 <= x <= 5)

* Improve code readability

* Remove accidentally committed change to Endstop.h

* Fix missing initialization and deduplicate code

* Use GCodeResult::error instead of GCodeResult::badOrMissingParameter

* Fix filename in comment, add description and revert changes in RepRapFirmware.h

Author: wilriker

src/Endstops/EndstopsManager.cpp

@@ -91,12 +91,13 @@ void EndstopsManager::EnableAxisEndstops(AxesBitmap axes, bool forHoming)
 }
 
 // Set up the active endstops for Z probing
-void EndstopsManager::EnableZProbe(size_t probeNumber)
+void EndstopsManager::EnableZProbe(size_t probeNumber, bool probingAway)
 {
 	activeEndstops = nullptr;
 	isHomingMove = false;
 	if (probeNumber < MaxZProbes && zProbes[probeNumber] != nullptr)
 	{
+		zProbes[probeNumber]->SetProbingAway(probingAway);
 		AddToActive(*zProbes[probeNumber]);
 	}
 }

src/Endstops/EndstopsManager.h

@@ -30,10 +30,10 @@ class EndstopsManager
 	void EnableAxisEndstops(AxesBitmap axes, bool forHoming);
 
 	// Set up the active endstops for Z probing
-	void EnableZProbe(size_t probeNumber);
+	void EnableZProbe(size_t probeNumber, bool probingAway = false);
 
 	// Set up the active endstops for Z probing with the current probe
-	void EnableCurrentZProbe() { EnableZProbe(currentZProbeNumber); }
+	void EnableCurrentZProbe(bool probingAway = false) { EnableZProbe(currentZProbeNumber, probingAway); }
 
 	// Enable extruder endstops
 	void EnableExtruderEndstop(size_t extruder);
@@ -56,6 +56,7 @@ class EndstopsManager
 	GCodeResult HandleG31(GCodeBuffer& gb, const StringRef& reply);			// G31
 
 	ZProbe& GetCurrentZProbe() const;
+	const size_t GetCurrentZProbeNumber() const { return currentZProbeNumber; }
 	ZProbe *GetZProbe(size_t num) const;
 	void SetZProbeDefaults();
 	GCodeResult ProgramZProbe(GCodeBuffer& gb, const StringRef& reply);

src/Endstops/ZProbe.cpp

@@ -32,7 +32,7 @@ void ZProbe::SetDefaults()
 	recoveryTime = 0.0;
 	tolerance = DefaultZProbeTolerance;
 	misc.parts.maxTaps = DefaultZProbeTaps;
-	misc.parts.invertReading = misc.parts.turnHeatersOff = misc.parts.saveToConfigOverride = false;
+	misc.parts.invertReading = misc.parts.turnHeatersOff = misc.parts.saveToConfigOverride = misc.parts.probingAway = false;
 	type = ZProbeType::none;
 	sensor = -1;
 }
@@ -136,7 +136,21 @@ EndStopHit ZProbe::Stopped() const
 EndstopHitDetails ZProbe::CheckTriggered(bool goingSlow)
 {
 	EndstopHitDetails rslt;
-	switch (Stopped())
+	EndStopHit e = Stopped();
+
+	// Note: This might need to be moved into Stopped() to not having to duplicate it to ZProbeEndstop
+	if (misc.parts.probingAway) {
+		switch (e) {
+		case EndStopHit::atStop:
+			e = EndStopHit::noStop;
+			break;
+		default:
+			e = EndStopHit::atStop;
+			break;
+		}
+	}
+
+	switch (e)
 	{
 	case EndStopHit::atStop:
 		rslt.SetAction(EndstopHitAction::stopAll);

src/Endstops/ZProbe.h

@@ -44,6 +44,7 @@ class ZProbe : public EndstopOrZProbe
 	bool GetSaveToConfigOverride() const { return misc.parts.saveToConfigOverride; }
 	int GetAdcValue() const { return adcValue; }
 	unsigned int GetMaxTaps() const { return misc.parts.maxTaps; }
+	void SetProbingAway(const bool probingAway) { misc.parts.probingAway = probingAway; }
 
 	int GetReading() const;
 	int GetSecondaryValues(int& v1, int& v2);
@@ -67,10 +68,11 @@ class ZProbe : public EndstopOrZProbe
 	{
 		struct
 		{
-			uint16_t maxTaps : 5,		// maximum probes at each point
-				invertReading : 1,		// true if we need to invert the reading
-				turnHeatersOff : 1,		// true to turn heaters off while probing
-				saveToConfigOverride : 1; // true if the trigger height should be saved to config-override.g
+			uint16_t maxTaps : 5,			// maximum probes at each point
+				invertReading : 1,			// true if we need to invert the reading
+				probingAway : 1,			// true if we are probing away, i.e. until contact lost
+				turnHeatersOff : 1,			// true to turn heaters off while probing
+				saveToConfigOverride : 1;	// true if the trigger height should be saved to config-override.g
 		} parts;
 		uint16_t all;
 	} misc;

src/GCodes/GCodeMachineState.h

@@ -80,6 +80,12 @@ enum class GCodeState : uint8_t
 	probingAtPoint6,
 	probingAtPoint7,
 
+	// These next 4 must be contiguous
+	straightProbe0,
+	straightProbe1,
+	straightProbe2,
+	straightProbe3,
+
 	doingFirmwareRetraction,
 	doingFirmwareUnRetraction,
 	loadingFilament,

src/GCodes/GCodes.cpp

@@ -2504,18 +2504,23 @@ MessageType GCodes::GetMessageBoxDevice(GCodeBuffer& gb) const
 	return mt;
 }
 
-// Do a manual bed probe. On entry the state variable is the state we want to return to when the user has finished adjusting the height.
-void GCodes::DoManualProbe(GCodeBuffer& gb)
+void GCodes::DoManualProbe(GCodeBuffer& gb, const char *message, const char *title, const AxesBitmap axes)
 {
 	if (Push(gb, true))													// stack the machine state including the file position and set the state to GCodeState::normal
 	{
 		gb.MachineState().CloseFile();									// stop reading from file if we were
 		gb.MachineState().waitingForAcknowledgement = true;				// flag that we are waiting for acknowledgement
 		const MessageType mt = GetMessageBoxDevice(gb);
-		platform.SendAlert(mt, "Adjust height until the nozzle just touches the bed, then press OK", "Manual bed probing", 2, 0.0, MakeBitmap<AxesBitmap>(Z_AXIS));
+		platform.SendAlert(mt, message, title, 2, 0.0, axes);
 	}
 }
 
+// Do a manual bed probe. On entry the state variable is the state we want to return to when the user has finished adjusting the height.
+void GCodes::DoManualBedProbe(GCodeBuffer& gb)
+{
+	DoManualProbe(gb, "Adjust height until the nozzle just touches the bed, then press OK", "Manual bed probing", MakeBitmap<AxesBitmap>(Z_AXIS));
+}
+
 // Start probing the grid, returning true if we didn't because of an error.
 // Prior to calling this the movement system must be locked.
 GCodeResult GCodes::ProbeGrid(GCodeBuffer& gb, const StringRef& reply)

src/GCodes/GCodes.h

@@ -259,6 +259,7 @@ class GCodes INHERIT_OBJECT_MODEL
 	void SetBedEquationWithProbe(int sParam, const StringRef& reply);			// Probes a series of points and sets the bed equation
 	GCodeResult SetOrReportOffsets(GCodeBuffer& gb, const StringRef& reply);	// Deal with a G10
 	GCodeResult SetPositions(GCodeBuffer& gb);									// Deal with a G92
+	GCodeResult StraightProbe(GCodeBuffer& gb, const StringRef& reply);			// Deal with a G38.x
 	GCodeResult DoDriveMapping(GCodeBuffer& gb, const StringRef& reply);		// Deal with a M584
 	GCodeResult ProbeTool(GCodeBuffer& gb, const StringRef& reply);				// Deal with a M585
 	GCodeResult FindCenterOfCavity(GCodeBuffer& gb, const StringRef& reply, const bool towardsMin = true);	// Deal with a M675
@@ -350,7 +351,8 @@ class GCodes INHERIT_OBJECT_MODEL
 	void CopyConfigFinalValues(GCodeBuffer& gb);								// Copy the feed rate etc. from the daemon to the input channels
 
 	MessageType GetMessageBoxDevice(GCodeBuffer& gb) const;						// Decide which device to display a message box on
-	void DoManualProbe(GCodeBuffer& gb);										// Do a manual bed probe
+	void DoManualProbe(GCodeBuffer&, const char *message, const char *title, const AxesBitmap); 			// Do manual probe in arbitrary direction
+	void DoManualBedProbe(GCodeBuffer& gb);										// Do a manual bed probe
 
 	void AppendAxes(const StringRef& reply, AxesBitmap axes) const;				// Append a list of axes to a string
 

src/GCodes/GCodes2.cpp

@@ -304,6 +304,14 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply)
 		DoFileMacro(gb, BED_EQUATION_G, true, 32);	// Try to execute bed.g
 		break;
 
+	case 38: // Straight probe - move until either the probe is triggered or the commanded move ends
+		if (!LockMovementAndWaitForStandstill(gb))
+		{
+			return false;
+		}
+		result = StraightProbe(gb, reply);
+		break;
+
 	case 53:	// Temporarily use machine coordinates
 		gb.MachineState().g53Active = true;
 		break;

src/GCodes/GCodes3.cpp

@@ -6,6 +6,7 @@
  *  This file contains functions that are called form file GCodes2.cpp to execute various G and M codes.
  */
 
+#include <Movement/StraightProbeSettings.h>
 #include "GCodes.h"
 
 #include "GCodeBuffer/GCodeBuffer.h"
@@ -676,6 +677,105 @@ GCodeResult GCodes::DoDriveMapping(GCodeBuffer& gb, const StringRef& reply)
 
 	return GCodeResult::ok;
 }
+// Handle G38.[2-5]
+GCodeResult GCodes::StraightProbe(GCodeBuffer& gb, const StringRef& reply)
+{
+	const int8_t fraction = gb.GetCommandFraction();
+	if (fraction < 2 || fraction > 5) {
+		return GCodeResult::warningNotSupported;
+	}
+	/*
+	 * It is an error if:
+	 * # the current point is the same as the programmed point.
+	 * # no axis word is used
+	 * # the feed rate is zero
+	 * # the probe is already in the target state
+	 */
+
+	StraightProbeSettings& sps = reprap.GetMove().GetStraightProbeSettings();
+	sps.Reset();
+
+
+	switch (fraction)
+	{
+	case 2:
+		sps.SetStraightProbeType(StraightProbeType::towardsWorkpieceErrorOnFailure);
+		break;
+
+	case 3:
+		sps.SetStraightProbeType(StraightProbeType::towardsWorkpiece);
+		break;
+
+	case 4:
+		sps.SetStraightProbeType(StraightProbeType::awayFromWorkpieceErrorOnFailure);
+		break;
+
+	case 5:
+		sps.SetStraightProbeType(StraightProbeType::awayFromWorkpiece);
+		break;
+	}
+
+	// Get the target coordinates and check if we would move at all
+	float target[MaxAxes];
+	ToolOffsetTransform(currentUserPosition, target);
+	bool seen = false;
+	bool doesMove = false;
+	for (size_t axis = 0; axis < MaxAxes; axis++)
+	{
+		if (gb.Seen(axisLetters[axis]))
+		{
+			seen = true;
+			const float axisTarget = gb.GetFValue();
+			if (axisTarget != target[axis])
+			{
+				doesMove = true;
+			}
+			target[axis] = axisTarget;
+			sps.AddMovingAxis(axis);
+		}
+	}
+
+	// No axis letters seen
+	if (!seen)
+	{
+		// Signal error for G38.2 and G38.4
+		if (sps.SignalError())
+		{
+			reply.copy("No axis specified.");
+			return GCodeResult::error;
+		}
+		return GCodeResult::ok;
+	}
+
+	// At least one axis seen but it would not result in movement
+	else if (!doesMove)
+	{
+		// Signal error for G38.2 and G38.4
+		if (sps.SignalError())
+		{
+			reply.copy("Target equals current position.");
+			return GCodeResult::error;
+		}
+		return GCodeResult::ok;
+	}
+	sps.SetTarget(target);
+
+	// See whether we are using a user-defined Z probe or just current one
+	size_t probeToUse = platform.GetEndstops().GetCurrentZProbeNumber();
+	if (gb.Seen('P'))
+	{
+		probeToUse = gb.GetUIValue();
+		if (platform.GetEndstops().GetZProbe(probeToUse) == nullptr)
+		{
+			reply.copy("Invalid probe number");
+			return GCodeResult::error;
+		}
+	}
+	sps.SetZProbeToUse(probeToUse);
+
+	gb.SetState(GCodeState::straightProbe0);
+	return GCodeResult::ok;
+}
 
 // Deal with a M585
 GCodeResult GCodes::ProbeTool(GCodeBuffer& gb, const StringRef& reply)