Fix to motion execution

3 files changed, 13 insertions, 18 deletions

diff --git a/src/Movement/AxisShaper.cpp b/src/Movement/AxisShaper.cpp
index cb64af4b..64738915 100644
--- a/src/Movement/AxisShaper.cpp
+++ b/src/Movement/AxisShaper.cpp
@@ -51,9 +51,9 @@ GCodeResult AxisShaper::Configure(GCodeBuffer& gb, const StringRef& reply) THROW
 	constexpr float MaximumInputShapingFrequency = 1000.0;
 	bool seen = false;
 
-	// If we are changing the type, frequency, damping or custom parameters, we will change multiple stored values used by the motoin planner, so wait until movement has stopped.
+	// If we are changing the type, frequency, damping or custom parameters, we will change multiple stored values used by the motion planner, so wait until movement has stopped.
 	// Changing just the minimum acceleration is OK because no other variables depend on it.
-	if (gb.Seen('F') || gb.Seen('S') || gb.Seen('P') || gb.Seen('H') || gb.Seen('T'))
+	if (gb.SeenAny("FSPHT"))
 	{
 		if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
 		{
diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp
index fca69228..4bf8d6f3 100644
--- a/src/Movement/DriveMovement.cpp
+++ b/src/Movement/DriveMovement.cpp
@@ -210,7 +210,6 @@ bool DriveMovement::NewExtruderSegment() noexcept
 		{
 			// Set up pB, pC such that for forward motion, time = pB + pC * stepNumber
 			pB = currentSegment->CalcLinearB(startDistance, startTime);
-			phaseStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 : (uint32_t)(distanceSoFar * mp.cart.effectiveStepsPerMm) + 1;
 			state = DMState::cartLinear;
 		}
 		else
@@ -222,17 +221,16 @@ bool DriveMovement::NewExtruderSegment() noexcept
 			{
 				// Extruders have a single acceleration segment. We need to add the extra extrusion distance due to pressure advance to the extrusion distance.
 				distanceSoFar += mp.cart.extraExtrusionDistance;
-				phaseStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 : (uint32_t)(distanceSoFar * mp.cart.effectiveStepsPerMm) + 1;
 				state = DMState::cartAccel;
 			}
 			else
 			{
-				// This is a decelerating segment. If it includes pressure advance then it may include reversal.
-				phaseStepLimit = totalSteps + 1;						// there is only one decelerating segment for extruders and it is at the end
+				// This is the single decelerating segment. If it includes pressure advance then it may include reversal.
 				state = DMState::cartDecelForwardsReversing;			// assume that it may reverse
 			}
 		}
 
+		phaseStepLimit = ((currentSegment->GetNext() == nullptr) ? totalSteps : (uint32_t)(distanceSoFar * mp.cart.effectiveStepsPerMm)) + 1;
 		if (nextStep < phaseStepLimit)
 		{
 			return true;
@@ -395,7 +393,7 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no
 
 		if (decelSeg == nullptr)
 		{
-			forwardDistance += dda.totalDistance;
+			forwardDistance += dda.totalDistance;			// no deceleration segment
 			reverseDistance = 0.0;
 		}
 		else
@@ -439,8 +437,9 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no
 	if (reverseDistance > 0.0)
 	{
 		const float netDistance = forwardDistance - reverseDistance;
+		const int32_t iFwdSteps = (int32_t)forwardSteps;
 		const int32_t netSteps = (int32_t)(netDistance * mp.cart.effectiveStepsPerMm);
-		if (netSteps == 0 && forwardSteps <= 1.0)
+		if (netSteps == 0 && iFwdSteps <= 1)
 		{
 			// No movement at all, or one step forward and one step back which we will ignore
 			shaper.SetExtrusionPending(netDistance * dda.directionVector[drive]);
@@ -448,10 +447,9 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no
 		}
 
 		// Note, netSteps may be negative for e.g. a deceleration-only move
-		const int32_t iFwdSteps = (int32_t)forwardSteps;
 		if (netSteps > 0 && netSteps + 1 >= iFwdSteps)
 		{
-			// There are zero or one reverse steps. We ignore a single reverse step.
+			// There is at least one forward step are zero or one reverse steps. We ignore a single reverse step.
 			totalSteps = netSteps;
 			reverseStartStep = totalSteps + 1;			// no reverse phase
 		}
@@ -470,14 +468,10 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no
 			totalSteps = (uint32_t)forwardSteps;
 			shaper.SetExtrusionPending((forwardDistance - (float)totalSteps * mp.cart.effectiveMmPerStep) * dda.directionVector[drive]);
 		}
-		else if (forwardSteps <= -1.0)
-		{
-			totalSteps = (uint32_t)(-forwardSteps);
-			shaper.SetExtrusionPending((forwardDistance + (float)totalSteps * mp.cart.effectiveMmPerStep) * dda.directionVector[drive]);
-		}
 		else
 		{
-			// No steps at all
+			// No steps at all, or negative forward steps which I think should be impossible unless the steps/mm is changed
+			totalSteps = 0;
 			shaper.SetExtrusionPending(forwardDistance * dda.directionVector[drive]);
 			return false;
 		}
@@ -523,9 +517,10 @@ pre(nextStep <= totalSteps; stepsTillRecalc == 0)
 			nextStep += 100000000;								// so we can tell what happened in the debug print
 			return false;
 		}
+		stepsToLimit = phaseStepLimit - nextStep;
 	}
 
-	if (phaseStepLimit > reverseStartStep)
+	if (reverseStartStep < phaseStepLimit && nextStep < reverseStartStep)
 	{
 		stepsToLimit = reverseStartStep - nextStep;
 	}
diff --git a/src/Movement/MoveSegment.h b/src/Movement/MoveSegment.h
index bc7dbf1b..10917f74 100644
--- a/src/Movement/MoveSegment.h
+++ b/src/Movement/MoveSegment.h
@@ -68,7 +68,7 @@
  * We can summarise like this. For an acceleration or deceleration segment:
  *   t = ts + B + sqrt(A + C*n/(f*m))
  * where for an acceleration segment:
- *   B = -u/a + k
+ *   B = -u/a - k
  *   A = B^2 - 2*(S0 + p/f)/a = B^2 - C * (S0 + p/f)
  *   C = 2/a
  * and for a deceleration segment: