Fixes for multiple motion systems

9 files changed, 152 insertions, 50 deletions

diff --git a/src/GCodes/CollisionAvoider.cpp b/src/GCodes/CollisionAvoider.cpp
index f011cb6f..0d8b9983 100644
--- a/src/GCodes/CollisionAvoider.cpp
+++ b/src/GCodes/CollisionAvoider.cpp
@@ -39,19 +39,19 @@ void CollisionAvoider::ResetPositions(const float positions[], AxesBitmap whichP
 }
 
 // Set the parameters
-void CollisionAvoider::Set(int axisL, int axisH, float sep, const float positions[]) noexcept
+void CollisionAvoider::Set(int axisL, int axisH, float sep) noexcept
 {
 	lowerAxis = axisL;
 	upperAxis = axisH;
 	minSeparation = sep;
-	lowerAxisMax = positions[lowerAxis];
-	upperAxisMin = positions[upperAxis];
+	lowerAxisMax = -std::numeric_limits<float>::infinity();
+	upperAxisMin = std::numeric_limits<float>::infinity();
 }
 
 // If the new move doesn't risk a collision, update the position accumulators and return true; else return false
-bool CollisionAvoider::UpdatePositions(const float axisPositions[]) noexcept
+bool CollisionAvoider::UpdatePositions(const float axisPositions[], AxesBitmap axesHomed) noexcept
 {
-	if (IsValid())
+	if (IsValid() && axesHomed.IsBitSet(lowerAxis) && axesHomed.IsBitSet(upperAxis))
 	{
 		const float newLowerMax = max<float>(axisPositions[lowerAxis], lowerAxisMax);
 		const float newUpperMin = min<float>(axisPositions[upperAxis], upperAxisMin);
diff --git a/src/GCodes/CollisionAvoider.h b/src/GCodes/CollisionAvoider.h
index 27f60942..61487245 100644
--- a/src/GCodes/CollisionAvoider.h
+++ b/src/GCodes/CollisionAvoider.h
@@ -28,10 +28,10 @@ public:
 	void ResetPositions(const float positions[], AxesBitmap whichPositions) noexcept;
 
 	// If the new move doesn't risk a collision, update the position accumulators and return true; else return false
-	bool UpdatePositions(const float axisPositions[]) noexcept;
+	bool UpdatePositions(const float axisPositions[], AxesBitmap axesHomed) noexcept;
 
 	// Set the parameters
-	void Set(int axisL, int axisH, float sep, const float positions[]) noexcept;
+	void Set(int axisL, int axisH, float sep) noexcept;
 
 private:
 	float minSeparation;
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 93c4c96a..8eed8727 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -258,15 +258,14 @@ void GCodes::Reset() noexcept
 #endif
 
 #if SUPPORT_ASYNC_MOVES
-	reprap.GetMove().GetKinematics().GetAssumedInitialPosition(numVisibleAxes, lastKnownMachinePositions);
+	MovementState::GlobalInit(numVisibleAxes);
 #endif
 
-	for (MovementState& ms : moveStates)
+	for (unsigned int i = 0; i < NumMovementSystems; ++i)
 	{
-		ms.Reset();
-#if SUPPORT_ASYNC_MOVES
-		memcpyf(ms.coords, lastKnownMachinePositions, numVisibleAxes);
-#else
+		MovementState& ms = moveStates[i];
+		ms.Init(i);
+#if !SUPPORT_ASYNC_MOVES
 		reprap.GetMove().GetKinematics().GetAssumedInitialPosition(numVisibleAxes, ms.coords);
 #endif
 		ToolOffsetInverseTransform(ms);
@@ -1598,16 +1597,23 @@ bool GCodes::LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned in
 		const AxesBitmap ownedAxes = ms.GetAxesAndExtrudersOwned();
 
 		// Whenever we release axes, we must update lastKnownMachinePositions for those axes first so that whoever allocated them next gets the correct positions
-		move.GetPartialMachinePosition(lastKnownMachinePositions, ownedAxes, msNumber);
-
-		memcpyf(ms.coords, lastKnownMachinePositions, MaxAxes);
-		move.InverseAxisAndBedTransform(lastKnownMachinePositions, ms.currentTool);
+		ms.SaveOwnAxisCoordinates();
+		memcpyf(ms.coords, MovementState::GetLastKnownMachinePositions(), MaxAxes);
+		move.InverseAxisAndBedTransform(ms.coords, ms.currentTool);
 		UpdateUserPositionFromMachinePosition(gb, ms);
-		collisionChecker.ResetPositions(lastKnownMachinePositions, ownedAxes);
+		collisionChecker.ResetPositions(ms.coords, ownedAxes);
 
-		// Release the axes and extruders that this movement system owns
-		axesAndExtrudersMoved.ClearBits(ownedAxes);
-		ms.ReleaseOwnedAxesAndExtruders();
+		// Release the axes and extruders that this movement system owns, except those used by the current tool
+		if (ms.currentTool != nullptr)
+		{
+			const AxesBitmap currentToolAxes = ms.currentTool->GetXYAxesAndExtruders();
+			const AxesBitmap axesToRelease = ms.GetAxesAndExtrudersOwned() & ~currentToolAxes;
+			ms.ReleaseAxesAndExtruders(axesToRelease);
+		}
+		else
+		{
+			ms.ReleaseOwnedAxesAndExtruders();
+		}
 #else
 		UpdateCurrentUserPosition(gb);
 #endif
@@ -2142,7 +2148,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 		}
 																				// apply tool offset, baby stepping, Z hop and axis scaling
 #if SUPPORT_ASYNC_MOVES
-		if (!collisionChecker.UpdatePositions(ms.coords))
+		if (!collisionChecker.UpdatePositions(ms.coords, axesHomed))
 		{
 			gb.ThrowGCodeException("potential collision detected");
 		}
@@ -2505,7 +2511,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 
 #if SUPPORT_ASYNC_MOVES
 	// Check the final position for collisions. We check the intermediate positions as we go.
-	collisionChecker.UpdatePositions(ms.coords);
+	collisionChecker.UpdatePositions(ms.coords, axesHomed);
 #endif
 
 	if (reprap.GetMove().GetKinematics().LimitPosition(ms.coords, nullptr, numVisibleAxes, axesVirtuallyHomed, true, limitAxes) != LimitPositionResult::ok)
@@ -2809,7 +2815,7 @@ bool GCodes::ReadMove(unsigned int queueNumber, RawMove& m) noexcept
 			// Limit the end position at each segment. This is needed for arc moves on any printer, and for [segmented] straight moves on SCARA printers.
 			if (   reprap.GetMove().GetKinematics().LimitPosition(m.coords, nullptr, numVisibleAxes, axesVirtuallyHomed, true, limitAxes) != LimitPositionResult::ok
 #if SUPPORT_ASYNC_MOVES
-				|| !collisionChecker.UpdatePositions(m.coords)
+				|| !collisionChecker.UpdatePositions(m.coords, axesHomed)
 #endif
 			   )
 			{
@@ -4963,12 +4969,11 @@ const MovementState& GCodes::GetCurrentMovementState(const ObjectExplorationCont
 // This relies on cooperative scheduling between different GCodeBuffer objects
 void GCodes::AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap axes, ParameterLettersBitmap axLetters) THROWS(GCodeException)
 {
-	if ((axes & axesAndExtrudersMoved & ~ms.GetAxesAndExtrudersOwned()).IsNonEmpty())
+	if (!ms.AllocateAxes(axes, axLetters).IsEmpty())
 	{
+		//TODO report the lowest axis letter that is already allocated
 		gb.ThrowGCodeException("Axis is already used by a different motion system");
 	}
-	axesAndExtrudersMoved |= axes;
-	ms.AllocateAxes(axes, axLetters);
 }
 
 // Allocate additional axes by letter when we are doing a standard move. The axis letters are as in the GCode, before we account for coordinate rotation and axis mapping.
@@ -4990,6 +4995,7 @@ void GCodes::AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, Param
 	}
 # endif
 
+	ms.SaveOwnAxisCoordinates();
 	AxesBitmap newAxes;
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
@@ -5020,6 +5026,7 @@ void GCodes::AllocateAxisLetters(const GCodeBuffer& gb, MovementState& ms, Param
 // Allocate axes by letter when we are doing a special move. Do not update the map of owned axes letters.
 void GCodes::AllocateAxesDirectFromLetters(const GCodeBuffer& gb, MovementState& ms, ParameterLettersBitmap axLetters) THROWS(GCodeException)
 {
+	ms.SaveOwnAxisCoordinates();
 	AxesBitmap newAxes;
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
@@ -5109,7 +5116,7 @@ bool GCodes::DoSync(GCodeBuffer& gb) noexcept
 void GCodes::UpdateAllCoordinates(const GCodeBuffer& gb) noexcept
 {
 	const unsigned int msNumber = gb.GetOwnQueueNumber();
-	memcpyf(moveStates[msNumber].coords, lastKnownMachinePositions, MaxAxes);
+	memcpyf(moveStates[msNumber].coords, MovementState::GetLastKnownMachinePositions(), MaxAxes);
 	reprap.GetMove().InverseAxisAndBedTransform(moveStates[msNumber].coords, moveStates[msNumber].currentTool);
 	UpdateUserPositionFromMachinePosition(gb, moveStates[msNumber]);
 	reprap.GetMove().SetNewPosition(moveStates[msNumber].coords, true, msNumber);
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index 606eb36e..91cd4da2 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -728,9 +728,7 @@ private:
 	AxesBitmap axesToSenseLength;				// The axes on which we are performing axis length sensing
 
 #if SUPPORT_ASYNC_MOVES
-	AxesBitmap axesAndExtrudersMoved;			// axes and extruders that have moved since the last sync
 	CollisionAvoider collisionChecker;
-	float lastKnownMachinePositions[MaxAxes];
 #endif
 
 #if HAS_MASS_STORAGE
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index 04aadd9f..1a1385ff 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -118,8 +118,9 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 			ToolOffsetInverseTransform(ms);		// make sure the limits are reflected in the user position
 		}
 		reprap.GetMove().SetNewPosition(ms.coords, true, gb.GetActiveQueueNumber());
-		//TODO update the other move system too!
-
+#if SUPPORT_ASYNC_MOVES
+		ms.SaveOwnAxisCoordinates();
+#endif
 		if (!IsSimulating())
 		{
 			axesHomed |= reprap.GetMove().GetKinematics().AxesAssumedHomed(axesIncluded);
@@ -130,6 +131,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 			}
 			reprap.MoveUpdated();				// because we may have updated axesHomed or zDatumSetByProbing
 		}
+
 	}
 
 	return GCodeResult::ok;
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index 547fc3fb..1c69eff4 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -363,13 +363,50 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 				ms.currentTool = nullptr;
 				UpdateCurrentUserPosition(gb);			// the tool offset may have changed, so get the current position
 			}
-			gb.AdvanceState();
-			if (Tool::GetLockedTool(ms.newToolNumber).IsNotNull() && (ms.toolChangeParam & TPreBit) != 0)	// 2020-04-29: run tpre file even if not all axes have been homed
+
+#if SUPPORT_ASYNC_MOVES && PREALLOCATE_TOOL_AXES
+			// Whenever we release axes, we must update lastKnownMachinePositions for those axes first so that whoever allocated them next gets the correct positions
+			ms.SaveOwnAxisCoordinates();
+
+			ReadLockedPointer<Tool> newTool = Tool::GetLockedTool(ms.newToolNumber);
+			if (newTool.IsNull())
 			{
-				String<StringLength20> scratchString;
-				scratchString.printf("tpre%d.g", ms.newToolNumber);
-				DoFileMacro(gb, scratchString.c_str(), false, ToolChangeMacroCode);
+				// Release the axes and extruders that this movement system owns
+				ms.ReleaseOwnedAxesAndExtruders();
+			}
+			else
+			{
+				// Allocate the axes and extruders that the new tool uses, and release all others
+				const AxesBitmap newToolAxes = newTool->GetXYAxesAndExtruders();
+				const AxesBitmap axesToAllocate = newToolAxes & ~ms.GetAxesAndExtrudersOwned();
+				const AxesBitmap axesToRelease = ms.GetAxesAndExtrudersOwned() & ~newToolAxes;
+				ms.ReleaseAxesAndExtruders(axesToRelease);
+				try
+				{
+					AllocateAxes(gb, ms, axesToAllocate, ParameterLettersToBitmap("XY"));
+				}
+				catch (const GCodeException& exc)
+				{
+					// We failed to allocate the new axes/extruders that we need
+					// Release all axes and extruders that this movement system owns
+					ms.ReleaseOwnedAxesAndExtruders();
+					gb.LatestMachineState().SetError(exc);
+					gb.SetState(GCodeState::normal);
+					break;
+				}
+				newTool.Release();						// release the tool list lock before we run tpre
+#else
+			if (Tool::GetLockedTool(ms.newToolNumber).IsNotNull())
+			{
+#endif
+				if ((ms.toolChangeParam & TPreBit) != 0)	// 2020-04-29: run tpre file even if not all axes have been homed
+				{
+					String<StringLength20> scratchString;
+					scratchString.printf("tpre%d.g", ms.newToolNumber);
+					DoFileMacro(gb, scratchString.c_str(), false, ToolChangeMacroCode);
+				}
 			}
+			gb.AdvanceState();
 		}
 		break;
 
diff --git a/src/GCodes/GCodes5.cpp b/src/GCodes/GCodes5.cpp
index 79db28f4..e4c726ee 100644
--- a/src/GCodes/GCodes5.cpp
+++ b/src/GCodes/GCodes5.cpp
@@ -118,7 +118,7 @@ GCodeResult GCodes::CollisionAvoidance(GCodeBuffer& gb, const StringRef& reply)
 		{
 			std::swap(upperValue, lowerValue);
 			std::swap(upperAxisNumber, lowerAxisNumber);
-			collisionChecker.Set(lowerAxisNumber, upperAxisNumber, upperValue - lowerValue, GetMovementState(gb).coords);
+			collisionChecker.Set(lowerAxisNumber, upperAxisNumber, upperValue - lowerValue);
 		}
 	}
 	else if (lowerAxisNumber >= 0)
diff --git a/src/Movement/RawMove.cpp b/src/Movement/RawMove.cpp
index 2b30d5c0..368f0b8f 100644
--- a/src/Movement/RawMove.cpp
+++ b/src/Movement/RawMove.cpp
@@ -11,6 +11,8 @@
 #include <Platform/RepRap.h>
 #include <Platform/Platform.h>
 #include <Tools/Tool.h>
+#include <Movement/Move.h>
+#include <Movement/Kinematics/Kinematics.h>
 
 // Set up some default values in the move buffer for special moves, e.g. for Z probing and firmware retraction
 void RawMove::SetDefaults(size_t firstDriveToZero) noexcept
@@ -35,23 +37,46 @@ void RawMove::SetDefaults(size_t firstDriveToZero) noexcept
 	}
 }
 
+#if SUPPORT_ASYNC_MOVES
+
+AxesBitmap MovementState::axesAndExtrudersMoved;						// axes and extruders that are owned by any movement system
+float MovementState::lastKnownMachinePositions[MaxAxes];				// the last stored machine position of the axes
+
+/*static*/ void MovementState::GlobalInit(size_t numVisibleAxes) noexcept
+{
+	axesAndExtrudersMoved.Clear();
+	reprap.GetMove().GetKinematics().GetAssumedInitialPosition(numVisibleAxes, lastKnownMachinePositions);
+	for (size_t i = numVisibleAxes; i < MaxAxes; ++i)
+	{
+		lastKnownMachinePositions[i] = 0.0;
+	}
+}
+
+#endif
+
 float MovementState::GetProportionDone() const noexcept
 {
 	return (float)(totalSegments - segmentsLeft)/(float)totalSegments;
 }
 
-void MovementState::Reset() noexcept
+// Initialise this MovementState. If SUPPORT_ASYNC_MOVES is set then must call MovementState::GlobalInit before calling this.
+void MovementState::Init(unsigned int p_msNumber) noexcept
 {
+	msNumber = p_msNumber;
 	ClearMove();
 	filePos = noFilePosition;
 	codeQueue->Clear();
 	currentCoordinateSystem = 0;
 	pausedInMacro = false;
 
+#if SUPPORT_ASYNC_MOVES
+	memcpyf(coords, MovementState::GetLastKnownMachinePositions(), MaxAxesPlusExtruders);
+#else
 	for (float& f : coords)
 	{
 		f = 0.0;									// clear out all axis and extruder coordinates
 	}
+#endif
 
 	maxPrintingAcceleration = ConvertAcceleration(DefaultPrintingAcceleration);
 	maxTravelAcceleration = ConvertAcceleration(DefaultTravelAcceleration);
@@ -231,10 +256,37 @@ void MovementState::InitObjectCancellation() noexcept
 // When releasing axes we must also release the corresponding axis letters, because they serve as a cache
 void MovementState::ReleaseOwnedAxesAndExtruders() noexcept
 {
+	axesAndExtrudersMoved.ClearBits(axesAndExtrudersOwned);
 	axesAndExtrudersOwned.Clear();
 	ownedAxisLetters.Clear();
 }
 
+void MovementState::ReleaseAxesAndExtruders(AxesBitmap axesToRelease) noexcept
+{
+	axesAndExtrudersOwned &= ~axesToRelease;						// clear the axes/extruders we have been asked to release
+	axesAndExtrudersMoved.ClearBits(axesToRelease);					// remove them from the own axes/extruders
+	ownedAxisLetters.Clear();										// clear the cache of owned axis letters
+}
+
+AxesBitmap MovementState::AllocateAxes(AxesBitmap axes, ParameterLettersBitmap axisLetters) noexcept
+{
+	const AxesBitmap unAvailable = axes & ~axesAndExtrudersOwned & axesAndExtrudersMoved;
+	if (unAvailable.IsEmpty())
+	{
+		axesAndExtrudersMoved |= axes;
+		axesAndExtrudersOwned |= axes;
+		ownedAxisLetters |= axisLetters;
+	}
+	return unAvailable;
+}
+
+// Save the coordinates of axes we own to lastKnownMachinePositions
+void MovementState::SaveOwnAxisCoordinates() const noexcept
+{
+	reprap.GetMove().GetPartialMachinePosition(lastKnownMachinePositions, GetAxesAndExtrudersOwned(), msNumber);
+}
+
+
 void AsyncMove::SetDefaults() noexcept
 {
 	for (float& f : movements)
diff --git a/src/Movement/RawMove.h b/src/Movement/RawMove.h
index 429d6b35..5f71197b 100644
--- a/src/Movement/RawMove.h
+++ b/src/Movement/RawMove.h
@@ -81,15 +81,22 @@ class MovementState : public RawMove
 public:
 
 #if SUPPORT_ASYNC_MOVES
-	AxesBitmap GetAxesAndExtrudersOwned() const noexcept { return axesAndExtrudersOwned; }		// Get the axes and extruders that this movement system owns
-	ParameterLettersBitmap GetOwnedAxisLetters() const noexcept { return ownedAxisLetters; }	// Get the letters denoting axes that this movement system owns
-	void AllocateAxes(AxesBitmap axes, ParameterLettersBitmap axisLetters) noexcept;
+	static void GlobalInit(size_t numVisibleAxes) noexcept;
+	static const float *GetLastKnownMachinePositions() noexcept { return lastKnownMachinePositions; }
+	static AxesBitmap GetAxesAndExtrudersMoved() noexcept { return axesAndExtrudersMoved; }
+
+	AxesBitmap GetAxesAndExtrudersOwned() const noexcept { return axesAndExtrudersOwned; }	// Get the axes and extruders that this movement system owns
+	ParameterLettersBitmap GetOwnedAxisLetters() const noexcept { return ownedAxisLetters; } // Get the letters denoting axes that this movement system owns
+	AxesBitmap AllocateAxes(AxesBitmap axes, ParameterLettersBitmap axisLetters) noexcept;	// try to allocate the requested axes, if we can't then return the axes we can't allocate
 	void ReleaseOwnedAxesAndExtruders() noexcept;
+	void ReleaseAxesAndExtruders(AxesBitmap axesToRelease) noexcept;
 	void ReleaseAxisLetter(char letter) noexcept;											// stop claiming that we own an axis letter (if we do) but don't release the associated axis
+	void SaveOwnAxisCoordinates() const noexcept;											// save the coordinates of axes we own to lastKnownMachinePositions
 #endif
 
+	unsigned int GetMsNumber() const noexcept { return msNumber; }
 	float GetProportionDone() const noexcept;												// get the proportion of this whole move that has been completed, based on segmentsLeft and totalSegments
-	void Reset() noexcept;
+	void Init(unsigned int p_msNumber) noexcept;
 	void ChangeExtrusionFactor(unsigned int extruder, float multiplier) noexcept;			// change the extrusion factor of an extruder
 	const RestorePoint& GetRestorePoint(size_t n) const pre(n < NumTotalRestorePoints) { return restorePoints[n]; }
 	void ClearMove() noexcept;
@@ -181,20 +188,19 @@ public:
 	bool printingJustResumed;										// true if we have just restarted printing
 
 private:
+	unsigned int msNumber;
+
 #if SUPPORT_ASYNC_MOVES
 	AxesBitmap axesAndExtrudersOwned;								// axes and extruders that this movement system has moved since the last sync
-	ParameterLettersBitmap ownedAxisLetters;						// letters denoting axes that this movement system owns
+	ParameterLettersBitmap ownedAxisLetters;						// cache of letters denoting user axes for which the corresponding machine axes for the current tool are definitely owned
+
+	static AxesBitmap axesAndExtrudersMoved;						// axes and extruders that are owned by any movement system
+	static float lastKnownMachinePositions[MaxAxes];				// the last stored machine position of the axes
 #endif
 };
 
 #if SUPPORT_ASYNC_MOVES
 
-inline void MovementState::AllocateAxes(AxesBitmap axes, ParameterLettersBitmap axisLetters) noexcept
-{
-	axesAndExtrudersOwned |= axes;
-	ownedAxisLetters |= axisLetters;
-}
-
 // Stop claiming that we own an axis letter (if we do) but don't release the associated axis
 inline void MovementState::ReleaseAxisLetter(char letter) noexcept
 {