From 727efd33387dadd10e3552c1bc2310d00ea5443a Mon Sep 17 00:00:00 2001 From: David Crocker Date: Tue, 11 Oct 2022 15:25:41 +0100 Subject: Changes to multiple motion system support --- src/GCodes/CollisionAvoider.cpp | 15 ++++++++--- src/GCodes/CollisionAvoider.h | 2 +- src/GCodes/GCodes.cpp | 55 +++++++++++++++-------------------------- src/GCodes/GCodes.h | 2 +- 4 files changed, 33 insertions(+), 41 deletions(-) diff --git a/src/GCodes/CollisionAvoider.cpp b/src/GCodes/CollisionAvoider.cpp index 8c5c879e..7e095951 100644 --- a/src/GCodes/CollisionAvoider.cpp +++ b/src/GCodes/CollisionAvoider.cpp @@ -23,12 +23,18 @@ bool CollisionAvoider::IsValid() const noexcept } // Reset the position accumulators -void CollisionAvoider::ResetPositions(const float positions[]) noexcept +void CollisionAvoider::ResetPositions(const float positions[], AxesBitmap whichPositions) noexcept { if (IsValid()) { - lowerAxisMax = positions[lowerAxis]; - upperAxisMin = positions[upperAxis]; + if (whichPositions.IsBitSet(lowerAxis)) + { + lowerAxisMax = positions[lowerAxis]; + } + if (whichPositions.IsBitSet(upperAxis)) + { + upperAxisMin = positions[upperAxis]; + } } } @@ -38,7 +44,8 @@ void CollisionAvoider::Set(int axisL, int axisH, float sep, const float position lowerAxis = axisL; upperAxis = axisH; minSeparation = sep; - ResetPositions(positions); + lowerAxisMax = positions[lowerAxis]; + upperAxisMin = positions[upperAxis]; } bool CollisionAvoider::UpdatePositions(const float axisPositions[]) noexcept diff --git a/src/GCodes/CollisionAvoider.h b/src/GCodes/CollisionAvoider.h index a8fda29e..27f60942 100644 --- a/src/GCodes/CollisionAvoider.h +++ b/src/GCodes/CollisionAvoider.h @@ -25,7 +25,7 @@ public: float GetMinSeparation()const noexcept { return minSeparation; } // Reset the position accumulators - void ResetPositions(const float positions[]) noexcept; + 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; diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp index cf3b7b7b..e3902540 100644 --- a/src/GCodes/GCodes.cpp +++ b/src/GCodes/GCodes.cpp @@ -1593,23 +1593,16 @@ bool GCodes::LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned in #if SUPPORT_ASYNC_MOVES // Get the position of all axes by combining positions from the queues Move& move = reprap.GetMove(); - float coords[MaxAxes]; - move.GetPartialMachinePosition(coords, AxesBitmap::MakeLowestNBits(numTotalAxes), 0); - move.GetPartialMachinePosition(coords, moveStates[1].axesAndExtrudersOwned, 1); - memcpyf(moveStates[0].coords, coords, MaxAxes); - memcpyf(moveStates[1].coords, coords, MaxAxes); - move.InverseAxisAndBedTransform(moveStates[0].coords, moveStates[0].currentTool); - move.InverseAxisAndBedTransform(moveStates[1].coords, moveStates[1].currentTool); - UpdateUserPositionFromMachinePosition(gb, moveStates[0]); //TODO problem when using coordinate rotation! - UpdateUserPositionFromMachinePosition(gb, moveStates[1]); - move.SetNewPosition(coords, true, 0); - move.SetNewPosition(coords, true, 1); - - // Release all axes and extruders - axesAndExtrudersMoved.Clear(); - moveStates[0].axesAndExtrudersOwned.Clear(); - moveStates[1].axesAndExtrudersOwned.Clear(); - collisionChecker.ResetPositions(moveStates[0].coords); + const AxesBitmap ownedAxes = moveStates[msNumber].axesAndExtrudersOwned; + move.GetPartialMachinePosition(lastKnownMachinePositions, ownedAxes, msNumber); + memcpyf(moveStates[msNumber].coords, lastKnownMachinePositions, MaxAxes); + move.InverseAxisAndBedTransform(lastKnownMachinePositions, moveStates[msNumber].currentTool); + UpdateUserPositionFromMachinePosition(gb, moveStates[msNumber]); + collisionChecker.ResetPositions(lastKnownMachinePositions, ownedAxes); + + // Release the axes and extruders that this movement system owns + axesAndExtrudersMoved.ClearBits(ownedAxes); + moveStates[msNumber].axesAndExtrudersOwned.Clear(); #else UpdateCurrentUserPosition(gb); #endif @@ -4950,7 +4943,7 @@ bool GCodes::SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept if (otherGb.IsLaterThan(thisGb)) { // Other input channel has skipped this sync point - UpdateUserCoordinatesAndReleaseOwnedAxes(thisGb, otherGb); // it's arguable whether updating machine coordinates from the other channel is worth doing here + UpdateAllCoordinates(thisGb); thisGb.syncState = GCodeBuffer::SyncState::running; return true; } @@ -4959,7 +4952,7 @@ bool GCodes::SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept } // If we get here then the other input channel is also syncing, so it's safe to use the machine axis coordinates of the axes it owns to update our user coordinates - UpdateUserCoordinatesAndReleaseOwnedAxes(thisGb, otherGb); + UpdateAllCoordinates(thisGb); // Now that we no longer need to read axis coordinates from the other motion system, flag that we have finished syncing thisGb.syncState = GCodeBuffer::SyncState::synced; @@ -4995,27 +4988,19 @@ bool GCodes::SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept // Synchronise the other motion system with this one. Return true if done, false if we need to wait for it to catch up. bool GCodes::DoSync(GCodeBuffer& gb) noexcept { - return (&gb == FileGCode()) ? SyncWith(gb, *File2GCode()) + const bool rslt = (&gb == FileGCode()) ? SyncWith(gb, *File2GCode()) : (&gb == File2GCode()) ? SyncWith(gb, *FileGCode()) : true; + return rslt; } -void GCodes::UpdateUserCoordinatesAndReleaseOwnedAxes(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept +void GCodes::UpdateAllCoordinates(GCodeBuffer& gb) noexcept { - // Get the position of all axes by combining positions from the queues - const MovementState& otherMs = GetConstMovementState(otherGb); - Move& move = reprap.GetMove(); - float coords[MaxAxes]; - move.GetPartialMachinePosition(coords, AxesBitmap::MakeLowestNBits(numTotalAxes), thisGb.GetOwnQueueNumber()); - move.GetPartialMachinePosition(coords, otherMs.axesAndExtrudersOwned, otherGb.GetOwnQueueNumber()); - MovementState& thisMs = GetMovementState(thisGb); - move.InverseAxisAndBedTransform(coords, thisMs.currentTool); - UpdateUserPositionFromMachinePosition(thisGb, thisMs); - move.SetNewPosition(coords, true, thisGb.GetOwnQueueNumber()); - - // Release all axes and extruders that we were using - axesAndExtrudersMoved &= ~thisMs.axesAndExtrudersOwned; - thisMs.axesAndExtrudersOwned.Clear(); + const unsigned int msNumber = gb.GetOwnQueueNumber(); + memcpyf(moveStates[msNumber].coords, lastKnownMachinePositions, MaxAxes); + reprap.GetMove().InverseAxisAndBedTransform(moveStates[msNumber].coords, moveStates[msNumber].currentTool); + UpdateUserPositionFromMachinePosition(gb, moveStates[msNumber]); + reprap.GetMove().SetNewPosition(moveStates[msNumber].coords, true, msNumber); } #endif diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h index d8952f57..e168da75 100644 --- a/src/GCodes/GCodes.h +++ b/src/GCodes/GCodes.h @@ -555,7 +555,7 @@ private: void AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap axes) THROWS(GCodeException); // allocate axes to a movement state bool DoSync(GCodeBuffer& gb) noexcept; // sync with the other stream returning true if done, false if we need to wait for it bool SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept; // synchronise motion systems - void UpdateUserCoordinatesAndReleaseOwnedAxes(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept; + void UpdateAllCoordinates(GCodeBuffer& gb) noexcept; #endif #if SUPPORT_COORDINATE_ROTATION -- cgit v1.2.3