From 4645654907115df006716d2dd208936525579bd9 Mon Sep 17 00:00:00 2001 From: David Crocker Date: Thu, 10 Nov 2022 19:52:05 +0000 Subject: Fixed G92 --- src/GCodes/GCodes3.cpp | 13 +++++++++---- src/GCodes/GCodes4.cpp | 5 ++++- src/Movement/RawMove.cpp | 18 ++++++++++-------- src/Movement/RawMove.h | 3 ++- 4 files changed, 25 insertions(+), 14 deletions(-) diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp index 51db8901..a511b714 100644 --- a/src/GCodes/GCodes3.cpp +++ b/src/GCodes/GCodes3.cpp @@ -79,6 +79,10 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS axisLettersMentioned.ClearBits(ms.GetOwnedAxisLetters()); if (axisLettersMentioned.IsNonEmpty()) { + if (!LockCurrentMovementSystemAndWaitForStandstill(gb)) // lock movement and get current coordinates before we try to allocate any axes + { + return GCodeResult::notFinished; + } AllocateAxisLetters(gb, ms, axisLettersMentioned); } #endif @@ -91,6 +95,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS if (gb.Seen(axisLetters[axis])) { const float axisValue = gb.GetFValue(); +#if !SUPPORT_ASYNC_MOVES if (axesIncluded.IsEmpty()) { if (!LockCurrentMovementSystemAndWaitForStandstill(gb)) // lock movement and get current coordinates @@ -98,6 +103,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS return GCodeResult::notFinished; } } +#endif axesIncluded.SetBit(axis); ms.currentUserPosition[axis] = gb.ConvertDistance(axisValue); } @@ -115,12 +121,12 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS if (reprap.GetMove().GetKinematics().LimitPosition(ms.coords, nullptr, numVisibleAxes, axesIncluded, false, limitAxes) != LimitPositionResult::ok) { - ToolOffsetInverseTransform(ms); // make sure the limits are reflected in the user position + ToolOffsetInverseTransform(ms); // make sure the limits are reflected in the user position } - reprap.GetMove().SetNewPosition(ms.coords, true, gb.GetActiveQueueNumber()); #if SUPPORT_ASYNC_MOVES - ms.SaveOwnAxisCoordinates(); + ms.OwnedAxisCoordinatesUpdated(axesIncluded); // save coordinates of any owned axes we changed #endif + reprap.GetMove().SetNewPosition(ms.coords, true, gb.GetActiveQueueNumber()); if (!IsSimulating()) { axesHomed |= reprap.GetMove().GetKinematics().AxesAssumedHomed(axesIncluded); @@ -131,7 +137,6 @@ 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 293bd405..51f06764 100644 --- a/src/GCodes/GCodes4.cpp +++ b/src/GCodes/GCodes4.cpp @@ -1120,7 +1120,10 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept if (tapsDone <= 1 && !hadProbingError) { // Reset the Z axis origin according to the height error so that we can move back up to the dive height - ms.UpdateOwnedAxisCoordinate(Z_AXIS, zp->GetActualTriggerHeight()); + ms.coords[Z_AXIS] = zp->GetActualTriggerHeight(); +#if SUPPORT_ASYNC_MOVES + ms.OwnedAxisCoordinateUpdated(Z_AXIS); +#endif reprap.GetMove().SetNewPosition(ms.coords, false, gb.GetActiveQueueNumber()); // Find the coordinates of the Z probe to pass to SetZeroHeightError diff --git a/src/Movement/RawMove.cpp b/src/Movement/RawMove.cpp index e0d68dc8..451817c6 100644 --- a/src/Movement/RawMove.cpp +++ b/src/Movement/RawMove.cpp @@ -293,16 +293,18 @@ void MovementState::SaveOwnAxisCoordinates() noexcept move.InverseAxisAndBedTransform(coords, currentTool); } +// Update changed coordinates of some owned axes - called after G92 +void MovementState::OwnedAxisCoordinatesUpdated(AxesBitmap axesIncluded) noexcept +{ + axesIncluded.Iterate([this](unsigned int bitNumber, unsigned int count)->void + { lastKnownMachinePositions[bitNumber] = coords[bitNumber]; } + ); +} + // Update the machine coordinate of an axis we own - called after Z probing -void MovementState::UpdateOwnedAxisCoordinate(size_t axis, float coordinate) noexcept +void MovementState::OwnedAxisCoordinateUpdated(size_t axis) noexcept { - coords[axis] = coordinate; -#if SUPPORT_ASYNC_MOVES - if (axesAndExtrudersOwned.IsBitSet(axis)) - { - lastKnownMachinePositions[axis] = coordinate; - } -#endif + lastKnownMachinePositions[axis] = coords[axis]; } void AsyncMove::SetDefaults() noexcept diff --git a/src/Movement/RawMove.h b/src/Movement/RawMove.h index fee725d0..9ac05b36 100644 --- a/src/Movement/RawMove.h +++ b/src/Movement/RawMove.h @@ -92,6 +92,8 @@ public: 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() noexcept; // fetch and save the coordinates of axes we own to lastKnownMachinePositions + void OwnedAxisCoordinatesUpdated(AxesBitmap axesIncluded) noexcept; // update changed coordinates of some owned axes - called after G92 + void OwnedAxisCoordinateUpdated(size_t axis) noexcept; // update the machine coordinate of an axis we own - called after Z probing #endif unsigned int GetMsNumber() const noexcept { return msNumber; } @@ -102,7 +104,6 @@ public: void ClearMove() noexcept; void SavePosition(unsigned int restorePointNumber, size_t numAxes, float p_feedRate, FilePosition p_filePos) noexcept pre(restorePointNumber < NumTotalRestorePoints); - void UpdateOwnedAxisCoordinate(size_t axis, float coordinate) noexcept; // update the machine coordinate of an axis we own - called after Z probing // Tool management void SelectTool(int toolNumber, bool simulating) noexcept; -- cgit v1.2.3