Implement M675 (#291)

5 files changed, 143 insertions, 0 deletions

diff --git a/src/GCodes/GCodeMachineState.h b/src/GCodes/GCodeMachineState.h
index 56eef265..d092cbbf 100644
--- a/src/GCodes/GCodeMachineState.h
+++ b/src/GCodes/GCodeMachineState.h
@@ -21,6 +21,10 @@ enum class GCodeState : uint8_t
 
 	probingToolOffset,
 
+	findCenterOfCavityMin,
+	findCenterOfCavityR,
+	findCenterOfCavityMax,
+
 	homing1,
 	homing2,
 
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 25006b27..7f1651ef 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -547,6 +547,75 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply)
 		}
 		break;
 
+	case GCodeState::findCenterOfCavityMin:
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			// We're trying to find the center of the cavity and we've moved all the way back until the corresponding
+			// endstop has been triggered. This means we can save the minimum position
+			SavePosition(findCenterOfCavityRestorePoint, gb);
+
+			// Move away from the endstop
+			const float rVal = gb.Seen('R') ? gb.GetFValue() : 5.0;
+			for (size_t axis = 0; axis < numVisibleAxes; ++axis)
+			{
+				if (gb.Seen(axisLetters[axis]))
+				{
+					for (size_t axs = 0; axs < numVisibleAxes; ++axs)
+					{
+						moveBuffer.coords[axs] = currentUserPosition[axs];
+					}
+					// Add R to the current position
+					moveBuffer.coords[axis] += rVal;
+
+					SetMoveBufferDefaults();
+					moveBuffer.feedRate = findCenterOfCavityRestorePoint.feedRate;
+					moveBuffer.canPauseAfter = false;
+					moveBuffer.hasExtrusion = false;
+
+					NewMoveAvailable(1);
+
+					break;
+				}
+			}
+			gb.SetState(GCodeState::findCenterOfCavityR);
+		}
+		break;
+
+	case GCodeState::findCenterOfCavityR:
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			// Kick off another probing move to the axis maximum
+			FindCenterOfCavity(gb, reply, false);
+		}
+		break;
+
+	case GCodeState::findCenterOfCavityMax:
+		if (LockMovementAndWaitForStandstill(gb))
+		{
+			// We get here when both the minimum and maximum values have been probed
+			for (size_t axis = 0; axis < numVisibleAxes; ++axis)
+			{
+				if (gb.Seen(axisLetters[axis]))
+				{
+					for (size_t axs = 0; axs < numVisibleAxes; ++axs)
+					{
+						moveBuffer.coords[axs] = findCenterOfCavityRestorePoint.moveCoords[axs];
+					}
+					moveBuffer.coords[axis] += (currentUserPosition[axis] - findCenterOfCavityRestorePoint.moveCoords[axis]) / 2;
+
+					SetMoveBufferDefaults();
+					moveBuffer.feedRate = findCenterOfCavityRestorePoint.feedRate;
+					moveBuffer.hasExtrusion = false;
+
+					gb.SetState(GCodeState::waitingForSpecialMoveToComplete);
+					NewMoveAvailable(1);
+
+					break;
+				}
+			}
+		}
+		break;
+
 	case GCodeState::homing1:
 		if (toBeHomed == 0)
 		{
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index 663734fd..dc2ead4d 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -307,6 +307,7 @@ private:
 	GCodeResult SetPositions(GCodeBuffer& gb);									// Deal with a G92
 	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
 	GCodeResult SetDateTime(GCodeBuffer& gb,const  StringRef& reply);			// Deal with a M905
 	GCodeResult SavePosition(GCodeBuffer& gb,const  StringRef& reply);			// Deal with G60
 	GCodeResult ConfigureDriver(GCodeBuffer& gb,const  StringRef& reply);		// Deal with M569
@@ -506,6 +507,7 @@ private:
 	RestorePoint numberedRestorePoints[NumRestorePoints];				// Restore points accessible using the R parameter in the G0/G1 command
 	RestorePoint& pauseRestorePoint = numberedRestorePoints[1];			// The position and feed rate when we paused the print
 	RestorePoint& toolChangeRestorePoint = numberedRestorePoints[2];	// The position and feed rate when we freed a tool
+	RestorePoint& findCenterOfCavityRestorePoint = numberedRestorePoints[3];	// The position and feed rate when we found the lower boundary of cavity
 
 	size_t numTotalAxes;						// How many axes we have
 	size_t numVisibleAxes;						// How many axes are visible
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 5ce93bec..80c3b1c2 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -3921,6 +3921,10 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply)
 		result = platform.ProgramZProbe(gb, reply);
 		break;
 
+	case 675:
+		result = FindCenterOfCavity(gb, reply);
+		break;
+
 	case 701: // Load filament
 		result = LoadFilament(gb, reply);
 		break;
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index 4f28d349..b136854c 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -899,6 +899,70 @@ GCodeResult GCodes::ProbeTool(GCodeBuffer& gb, const StringRef& reply)
 	return GCodeResult::ok;
 }
 
+GCodeResult GCodes::FindCenterOfCavity(GCodeBuffer& gb, const StringRef& reply, const bool towardsMin)
+{
+	if (reprap.GetCurrentTool() == nullptr)
+	{
+		reply.copy("No tool selected!");
+		return GCodeResult::error;
+	}
+
+	if (!LockMovementAndWaitForStandstill(gb))
+	{
+		return GCodeResult::notFinished;
+	}
+
+	for (size_t axis = 0; axis < numVisibleAxes; axis++)
+	{
+		if (gb.Seen(axisLetters[axis]))
+		{
+
+			SetMoveBufferDefaults();
+
+			// Prepare a move similar to G1 .. S3
+			moveBuffer.moveType = 3;
+			SetBit(moveBuffer.endStopsToCheck, axis);
+			axesToSenseLength = 0;
+
+			doingArcMove = false;
+
+			moveBuffer.canPauseAfter = false;
+			moveBuffer.hasExtrusion = false;
+
+			moveBuffer.coords[axis] = towardsMin ? platform.AxisMinimum(axis) : platform.AxisMaximum(axis);
+
+			// Deal with feed rate
+			if (gb.Seen(feedrateLetter))
+			{
+				const float rate = gb.ConvertDistance(gb.GetFValue());
+				gb.MachineState().feedRate = rate * SecondsToMinutes;	// don't apply the speed factor to homing and other special moves
+			}
+			else
+			{
+				reply.copy("No feed rate provided.");
+				return GCodeResult::badOrMissingParameter;
+			}
+			moveBuffer.feedRate = gb.MachineState().feedRate;
+
+			if (towardsMin)
+			{
+				gb.SetState(GCodeState::findCenterOfCavityMin);
+			}
+			else
+			{
+				gb.SetState(GCodeState::findCenterOfCavityMax);
+			}
+
+			// Kick off new movement
+			NewMoveAvailable(1);
+
+			// Only do one axis at a time
+			break;
+		}
+	}
+	return GCodeResult::ok;
+}
+
 // Deal with a M905
 GCodeResult GCodes::SetDateTime(GCodeBuffer& gb, const StringRef& reply)
 {