Reworked axis shaping to fix bugs

8 files changed, 216 insertions, 186 deletions

diff --git a/src/CAN/CanMotion.cpp b/src/CAN/CanMotion.cpp
index c98efcec..da0375a6 100644
--- a/src/CAN/CanMotion.cpp
+++ b/src/CAN/CanMotion.cpp
@@ -89,9 +89,9 @@ CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexce
 		if (buf->next == nullptr)
 		{
 			// This is the first CAN-connected board for this movement
-			move->accelerationClocks = (uint32_t)params.accelClocks;
-			move->steadyClocks = (uint32_t)params.steadyClocks;
-			move->decelClocks = (uint32_t)params.decelClocks;
+			move->accelerationClocks = (uint32_t)params.unshaped.accelClocks;
+			move->steadyClocks = (uint32_t)params.unshaped.steadyClocks;
+			move->decelClocks = (uint32_t)params.unshaped.decelClocks;
 			currentMoveClocks = move->accelerationClocks + move->steadyClocks + move->decelClocks;
 		}
 		else
diff --git a/src/Movement/AxisShaper.cpp b/src/Movement/AxisShaper.cpp
index 96132d0b..38baee72 100644
--- a/src/Movement/AxisShaper.cpp
+++ b/src/Movement/AxisShaper.cpp
@@ -410,7 +410,6 @@ void AxisShaper::PlanShaping(DDA& dda, PrepParams& params, bool shapingEnabled)
 						}
 						dda.deceleration = proposedDeceleration;
 						dda.topSpeed = dda.startSpeed;
-						dda.beforePrepare.accelDistance = 0.0;
 						dda.beforePrepare.decelDistance = dda.totalDistance;
 					}
 					else
@@ -443,27 +442,28 @@ void AxisShaper::PlanShaping(DDA& dda, PrepParams& params, bool shapingEnabled)
 		// Set up the provisional parameters
 		params.SetFromDDA(dda);
 
-		if (params.accelDistance < params.decelStartDistance)			// we can't do any shaping unless there is a steady speed segment that can be shortened
+		if (params.unshaped.accelDistance < params.unshaped.decelStartDistance)			// we can't do any shaping unless there is a steady speed segment that can be shortened
 		{
+			params.shaped = params.unshaped;
 			//TODO if we want to shape both acceleration and deceleration but the steady distance is zero or too short, we could reduce the top speed
-			if (params.accelDistance > 0.0)
+			if (params.unshaped.accelDistance > 0.0)
 			{
 				if ((dda.GetPrevious()->state != DDA::DDAState::frozen && dda.GetPrevious()->state != DDA::DDAState::executing) || !dda.GetPrevious()->flags.wasAccelOnlyMove)
 				{
 					TryShapeAccelBoth(dda, params);
 				}
-				else if (params.accelClocks >= minimumShapingEndOriginalClocks)
+				else if (params.unshaped.accelClocks >= minimumShapingEndOriginalClocks)
 				{
 					TryShapeAccelEnd(dda, params);
 				}
 			}
-			if (params.decelDistance > 0.0)
+			if (params.unshaped.decelDistance > 0.0)
 			{
 				if (dda.GetNext()->GetState() != DDA::DDAState::provisional || !dda.GetNext()->IsDecelerationMove())
 				{
 					TryShapeDecelBoth(dda, params);
 				}
-				else if (params.decelClocks >= minimumShapingStartOriginalClocks)
+				else if (params.unshaped.decelClocks >= minimumShapingStartOriginalClocks)
 				{
 					TryShapeDecelStart(dda, params);
 				}
@@ -477,31 +477,15 @@ void AxisShaper::PlanShaping(DDA& dda, PrepParams& params, bool shapingEnabled)
 	{
 		MoveSegment * const accelSegs = GetAccelerationSegments(dda, params);
 		MoveSegment * const decelSegs = GetDecelerationSegments(dda, params);
-		params.Finalise(dda);									// this sets up params.steadyClocks, which is needed by FinishSegments
-		dda.shapedSegments = FinishSegments(dda, params, accelSegs, decelSegs);
-
-		// Update the acceleration and deceleration in the DDA and the acceleration/deceleration distances and times in the PrepParams, so that if we generate unshaped segments or CAN motion too, they will be in sync
-		// Replace the shaped acceleration by a linear acceleration followed by constant speed time
-		if (params.shapingPlan.shapeAccelStart || params.shapingPlan.shapeAccelEnd || params.shapingPlan.shapeAccelOverlapped)
-		{
-			const float speedIncrease = dda.topSpeed - dda.startSpeed;
-			params.accelClocks = 2 * (dda.topSpeed * params.accelClocks - params.accelDistance)/speedIncrease;
-			params.accelDistance = (dda.startSpeed + dda.topSpeed) * params.accelClocks * 0.5;
-			dda.acceleration = speedIncrease/params.accelClocks;
-		}
-		if (params.shapingPlan.shapeDecelStart || params.shapingPlan.shapeDecelEnd || params.shapingPlan.shapeDecelOverlapped)
-		{
-			const float speedDecrease = dda.topSpeed - dda.endSpeed;
-			params.decelClocks = 2 * (dda.topSpeed * params.decelClocks - params.decelDistance)/speedDecrease;
-			params.decelDistance = (dda.topSpeed + dda.endSpeed) * params.decelClocks * 0.5;
-			params.decelStartDistance = dda.totalDistance - params.decelDistance;
-			dda.deceleration = speedDecrease/params.decelClocks;
-		}
-		params.steadyClocks = max<float>(dda.clocksNeeded - params.accelClocks - params.decelClocks, 0.0);
+		params.shaped.Finalise(dda.topSpeed);							// this sets up params.steadyClocks, which is needed by FinishShapedSegments
+		dda.clocksNeeded = params.shaped.TotalClocks();
+		dda.shapedSegments = FinishShapedSegments(dda, params, accelSegs, decelSegs);
+		params.unshaped.steadyClocks = max<float>(dda.clocksNeeded - params.unshaped.accelClocks - params.unshaped.decelClocks, 0.0);
 	}
 	else
 	{
-		params.Finalise(dda);									// this sets up params.steadyClocks
+		params.unshaped.Finalise(dda.topSpeed);									// this sets up params.steadyClocks
+		dda.clocksNeeded = params.unshaped.TotalClocks();
 	}
 
 //	debugPrintf(" final plan %03x\n", (unsigned int)params.shapingPlan.all);
@@ -510,12 +494,10 @@ void AxisShaper::PlanShaping(DDA& dda, PrepParams& params, bool shapingEnabled)
 // Try to shape the end of the acceleration. We already know that there is sufficient acceleration time to do this, but we still need to check that there is enough distance.
 void AxisShaper::TryShapeAccelEnd(const DDA& dda, PrepParams& params) const noexcept
 {
-	const float extraAccelDistance = GetExtraAccelEndDistance(dda);
-	if (params.accelDistance + extraAccelDistance <= params.decelStartDistance)
+	const float extraAccelDistance = GetExtraAccelEndDistance(dda.topSpeed, params.unshaped.acceleration);
+	if (ImplementAccelShaping(dda, params, params.unshaped.accelDistance + extraAccelDistance, params.unshaped.accelClocks + extraClocksAtEnd))
 	{
 		params.shapingPlan.shapeAccelEnd = true;
-		params.accelDistance += extraAccelDistance;
-		params.accelClocks += extraClocksAtEnd;
 	}
 	else
 	{
@@ -529,7 +511,7 @@ void AxisShaper::TryShapeAccelEnd(const DDA& dda, PrepParams& params) const noex
 
 void AxisShaper::TryShapeAccelBoth(DDA& dda, PrepParams& params) const noexcept
 {
-	if (dda.topSpeed - dda.startSpeed <= overlappedDeltaVPerA * dda.acceleration)
+	if (dda.topSpeed - dda.startSpeed <= overlappedDeltaVPerA * params.unshaped.acceleration)
 	{
 		// We can use overlapped shaping
 		const float newAcceleration = (dda.topSpeed - dda.startSpeed)/overlappedDeltaVPerA;
@@ -537,54 +519,94 @@ void AxisShaper::TryShapeAccelBoth(DDA& dda, PrepParams& params) const noexcept
 		{
 			return;
 		}
-		const float newAccelDistance = (dda.startSpeed * overlappedShapingClocks) + (newAcceleration * overlappedDistancePerA);
-		if (newAccelDistance >= params.decelStartDistance)
+
+		const float newAccelDistnce = (dda.startSpeed * overlappedShapingClocks) + (newAcceleration * overlappedDistancePerA);
+		if (ImplementAccelShaping(dda, params, newAccelDistnce, overlappedShapingClocks))
 		{
+			params.shapingPlan.shapeAccelOverlapped = true;
+			params.shaped.acceleration = newAcceleration;
 			return;
 		}
-		dda.acceleration = newAcceleration;
-		params.accelDistance = newAccelDistance;
-		params.accelClocks = overlappedShapingClocks;
-		params.shapingPlan.shapeAccelOverlapped = true;
 	}
 	else
 	{
-		if (params.accelClocks < minimumNonOverlappedOriginalClocks)
+		if (params.unshaped.accelClocks < minimumNonOverlappedOriginalClocks)
 		{
 			// The speed change is too high to allow overlapping, but non-overlapped shaping will give a very short steady acceleration segment.
 			// If we have enough spare distance, reduce the acceleration slightly to lengthen that segment.
-			const float newAcceleration = (dda.acceleration * params.accelClocks)/minimumNonOverlappedOriginalClocks;
+			const float newAcceleration = (params.unshaped.acceleration * params.unshaped.accelClocks)/minimumNonOverlappedOriginalClocks;
 			const float newAccelDistance = (dda.startSpeed + (0.5 * newAcceleration * minimumNonOverlappedOriginalClocks)) * minimumNonOverlappedOriginalClocks;
-			if (newAccelDistance >= params.decelStartDistance)
+			if (ImplementAccelShaping(dda, params, newAccelDistance, minimumNonOverlappedOriginalClocks))
 			{
-				return;
+				params.shapingPlan.shapeAccelStart = params.shapingPlan.shapeAccelEnd = true;
+				params.shaped.acceleration = newAcceleration;
 			}
-			dda.acceleration = newAcceleration;
-			params.accelDistance = newAccelDistance;
-			params.accelClocks = minimumNonOverlappedOriginalClocks;
 		}
-
-		// Only perform shaping if we can shape both the start and end of acceleration, otherwise we may not be able to generate a corresponding unshaped move because it might require negative steady distance
-		const float extraAccelDistance = GetExtraAccelStartDistance(dda) + GetExtraAccelEndDistance(dda);
-		if (params.accelDistance + extraAccelDistance <= params.decelStartDistance)
+		else
 		{
-			params.shapingPlan.shapeAccelStart = params.shapingPlan.shapeAccelEnd = true;
-			params.accelDistance += extraAccelDistance;
-			params.accelClocks += extraClocksAtStart + extraClocksAtEnd;
+			// We only attempt shaping if we can shape both the start and end of acceleration
+			const float extraAccelDistance = GetExtraAccelStartDistance(dda.startSpeed, params.unshaped.acceleration) + GetExtraAccelEndDistance(dda.topSpeed, params.unshaped.acceleration);
+			if (ImplementAccelShaping(dda, params, params.unshaped.accelDistance + extraAccelDistance, params.unshaped.accelClocks + extraClocksAtStart + extraClocksAtEnd))
+			{
+				params.shapingPlan.shapeAccelStart = params.shapingPlan.shapeAccelEnd = true;
+			}
+		}
+	}
+}
+
+// Check whether we can implement acceleration shaping using the proposed parameters; if so then implement it and return true; else return false with nothing changed
+bool AxisShaper::ImplementAccelShaping(const DDA& dda, PrepParams& params, float newAccelDistance, float newAccelClocks) const noexcept
+{
+	if (newAccelDistance <= params.shaped.decelStartDistance)
+	{
+		const float speedIncrease = dda.topSpeed - dda.startSpeed;
+		const float unshapedAccelClocks = 2 * (dda.topSpeed * newAccelClocks - newAccelDistance)/speedIncrease;
+		const float unshapedAccelDistance = (dda.startSpeed + dda.topSpeed) * unshapedAccelClocks * 0.5;
+		if (unshapedAccelDistance <= params.shaped.decelStartDistance)
+		{
+			params.shaped.accelDistance = newAccelDistance;
+			params.shaped.accelClocks = newAccelClocks;
+			params.unshaped.accelClocks = unshapedAccelClocks;
+			params.unshaped.accelDistance = unshapedAccelDistance;
+			params.unshaped.acceleration = speedIncrease/unshapedAccelClocks;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+// Check whether we can implement acceleration shaping using the proposed parameters; if so then implement it and return true; else return false with nothing changed
+bool AxisShaper::ImplementDecelShaping(const DDA& dda, PrepParams& params, float newDecelDistance, float newDecelClocks) const noexcept
+{
+	if (params.shaped.accelDistance + newDecelDistance <= dda.totalDistance)
+	{
+		const float speedDecrease = dda.topSpeed - dda.endSpeed;
+		const float unshapedDecelClocks = 2 * (dda.topSpeed * newDecelClocks - newDecelDistance)/speedDecrease;
+		const float unshapedDecelDistance = (dda.topSpeed + dda.endSpeed) * unshapedDecelClocks * 0.5;
+		if (params.unshaped.accelDistance + unshapedDecelDistance <= dda.totalDistance)
+		{
+			params.shaped.decelDistance = newDecelDistance;
+			params.shaped.decelStartDistance = dda.totalDistance - newDecelDistance;
+			params.shaped.decelClocks = newDecelClocks;
+			params.unshaped.decelClocks = unshapedDecelClocks;
+			params.unshaped.decelDistance = unshapedDecelDistance;
+			params.unshaped.decelStartDistance = dda.totalDistance - unshapedDecelDistance;
+			params.unshaped.deceleration = speedDecrease/unshapedDecelClocks;
+			return true;
 		}
 	}
+
+	return false;
 }
 
 // Try to shape the start of the deceleration. We already know that there is sufficient deceleration time to do this, but we still need to check that there is enough distance.
 void AxisShaper::TryShapeDecelStart(const DDA& dda, PrepParams& params) const noexcept
 {
-	float extraDecelDistance = GetExtraDecelStartDistance(dda);
-	if (params.accelDistance + extraDecelDistance <= params.decelStartDistance)
+	const float extraDecelDistance = GetExtraDecelStartDistance(dda.topSpeed, params.unshaped.deceleration);
+	if (ImplementDecelShaping(dda, params, params.unshaped.accelDistance + extraDecelDistance, params.unshaped.decelClocks + extraClocksAtStart))
 	{
 		params.shapingPlan.shapeDecelStart = true;
-		params.decelStartDistance -= extraDecelDistance;
-		params.decelDistance += extraDecelDistance;
-		params.decelClocks += extraClocksAtStart;
 	}
 	else
 	{
@@ -598,7 +620,7 @@ void AxisShaper::TryShapeDecelStart(const DDA& dda, PrepParams& params) const no
 
 void AxisShaper::TryShapeDecelBoth(DDA& dda, PrepParams& params) const noexcept
 {
-	if (dda.topSpeed - dda.endSpeed <= overlappedDeltaVPerA * dda.deceleration)
+	if (dda.topSpeed - dda.endSpeed <= overlappedDeltaVPerA * params.unshaped.deceleration)
 	{
 		// We can use overlapped shaping
 		const float newDeceleration = (dda.topSpeed - dda.endSpeed)/overlappedDeltaVPerA;
@@ -606,44 +628,36 @@ void AxisShaper::TryShapeDecelBoth(DDA& dda, PrepParams& params) const noexcept
 		{
 			return;
 		}
+
 		const float newDecelDistance = (dda.topSpeed * overlappedShapingClocks) - (newDeceleration * overlappedDistancePerA);
-		const float newDecelStartDistance = dda.totalDistance - newDecelDistance;
-		if (newDecelStartDistance < params.accelDistance)
+		if (ImplementDecelShaping(dda, params, newDecelDistance, overlappedShapingClocks))
 		{
-			return;
+			params.shapingPlan.shapeDecelOverlapped = true;
+			params.shaped.deceleration = newDeceleration;
 		}
-		dda.deceleration = newDeceleration;
-		params.decelDistance = newDecelDistance;
-		params.decelStartDistance = newDecelStartDistance;
-		params.decelClocks = overlappedShapingClocks;
-		params.shapingPlan.shapeDecelOverlapped = true;
 	}
 	else
 	{
-		if (params.decelClocks < minimumNonOverlappedOriginalClocks)
+		if (params.unshaped.decelClocks < minimumNonOverlappedOriginalClocks)
 		{
 			// The speed change is too high to allow overlapping, but non-overlapped shaping will give a very short steady acceleration segment.
 			// If we have enough spare distance, reduce the acceleration slightly to lengthen that segment.
-			const float newDeceleration = (dda.deceleration * params.decelClocks)/minimumNonOverlappedOriginalClocks;
+			const float newDeceleration = (params.unshaped.deceleration * params.unshaped.decelClocks)/minimumNonOverlappedOriginalClocks;
 			const float newDecelDistance = (dda.endSpeed + (0.5 * newDeceleration * minimumNonOverlappedOriginalClocks)) * minimumNonOverlappedOriginalClocks;
-			const float newDecelStartDistance = dda.totalDistance - newDecelDistance;
-			if (newDecelStartDistance <= params.accelDistance)
+			if (ImplementDecelShaping(dda, params, newDecelDistance, overlappedShapingClocks))
 			{
-				return;
+				params.shapingPlan.shapeDecelStart = params.shapingPlan.shapeDecelEnd = true;
+				params.shaped.deceleration = newDeceleration;
 			}
-			dda.deceleration = newDeceleration;
-			params.decelStartDistance = newDecelStartDistance;
-			params.decelClocks = overlappedShapingClocks;
 		}
-
-		// Only perform shaping if we can shape both the start and end of deceleration, otherwise we may not be able to generate a corresponding unshaped move because it might require negative steady distance
-		const float extraDecelDistance = GetExtraDecelStartDistance(dda) + GetExtraDecelEndDistance(dda);
-		if (params.accelDistance + extraDecelDistance <= params.decelStartDistance)
+		else
 		{
-			params.shapingPlan.shapeDecelStart = params.shapingPlan.shapeDecelEnd = true;
-			params.decelStartDistance -= extraDecelDistance;
-			params.decelDistance += extraDecelDistance;
-			params.decelClocks += extraClocksAtStart + extraClocksAtEnd;
+			// Only perform shaping if we can shape both the start and end of deceleration, otherwise we may not be able to generate a corresponding unshaped move because it might require negative steady distance
+			const float extraDecelDistance = GetExtraDecelStartDistance(dda.topSpeed, params.unshaped.deceleration) + GetExtraDecelEndDistance(dda.endSpeed, params.unshaped.deceleration);
+			if (ImplementDecelShaping(dda, params, params.unshaped.decelDistance + extraDecelDistance, params.unshaped.decelClocks + extraClocksAtStart + extraClocksAtEnd))
+			{
+				params.shapingPlan.shapeDecelStart = params.shapingPlan.shapeDecelEnd = true;
+			}
 		}
 	}
 }
@@ -660,7 +674,7 @@ MoveSegment *AxisShaper::GetAccelerationSegments(const DDA& dda, const PrepParam
 			for (int i = (2 * numExtraImpulses) - 1; i >= 0; --i)
 			{
 				accelSegs = MoveSegment::Allocate(accelSegs);
-				const float acceleration = dda.acceleration * overlappedCoefficients[i];
+				const float acceleration = params.shaped.acceleration * overlappedCoefficients[i];
 				const float segTime = overlappedDurations[i];
 				segStartSpeed -= acceleration * segTime;
 				const float b = segStartSpeed/(-acceleration);
@@ -672,7 +686,7 @@ MoveSegment *AxisShaper::GetAccelerationSegments(const DDA& dda, const PrepParam
 		}
 
 		float accumulatedSegTime = 0.0;
-		float endDistance = params.accelDistance;
+		float endDistance = params.shaped.accelDistance;
 		MoveSegment *endAccelSegs = nullptr;
 		if (params.shapingPlan.shapeAccelEnd)
 		{
@@ -681,7 +695,7 @@ MoveSegment *AxisShaper::GetAccelerationSegments(const DDA& dda, const PrepParam
 			for (int i = numExtraImpulses - 1; i >= 0; --i)
 			{
 				endAccelSegs = MoveSegment::Allocate(endAccelSegs);
-				const float acceleration = dda.acceleration * (1.0 - coefficients[i]);
+				const float acceleration = params.shaped.acceleration * (1.0 - coefficients[i]);
 				const float segTime = durations[i];
 				segStartSpeed -= acceleration * segTime;
 				const float b = segStartSpeed/(-acceleration);
@@ -702,7 +716,7 @@ MoveSegment *AxisShaper::GetAccelerationSegments(const DDA& dda, const PrepParam
 			for (unsigned int i = 0; i < numExtraImpulses; ++i)
 			{
 				MoveSegment *seg = MoveSegment::Allocate(nullptr);
-				const float acceleration = dda.acceleration * coefficients[i];
+				const float acceleration = params.shaped.acceleration * coefficients[i];
 				const float segTime = durations[i];
 				const float b = startSpeed/(-acceleration);
 				const float c = 2.0/acceleration;
@@ -726,9 +740,9 @@ MoveSegment *AxisShaper::GetAccelerationSegments(const DDA& dda, const PrepParam
 		if (endDistance > startDistance)
 		{
 			endAccelSegs = MoveSegment::Allocate(endAccelSegs);
-			const float b = startSpeed/(-dda.acceleration);
-			const float c = 2.0/dda.acceleration;
-			endAccelSegs->SetNonLinear(endDistance - startDistance, params.accelClocks - accumulatedSegTime, b, c);
+			const float b = startSpeed/(-params.shaped.acceleration);
+			const float c = 2.0/params.shaped.acceleration;
+			endAccelSegs->SetNonLinear(endDistance - startDistance, params.shaped.accelClocks - accumulatedSegTime, b, c);
 		}
 
 		if (startAccelSegs == nullptr)
@@ -758,7 +772,7 @@ MoveSegment *AxisShaper::GetDecelerationSegments(const DDA& dda, const PrepParam
 			for (int i = (2 * numExtraImpulses) - 1; i >= 0; --i)
 			{
 				decelSegs = MoveSegment::Allocate(decelSegs);
-				const float deceleration = dda.deceleration * overlappedCoefficients[i];
+				const float deceleration = params.shaped.deceleration * overlappedCoefficients[i];
 				const float segTime = overlappedDurations[i];
 				segStartSpeed += deceleration * segTime;
 				const float b = segStartSpeed/deceleration;
@@ -779,7 +793,7 @@ MoveSegment *AxisShaper::GetDecelerationSegments(const DDA& dda, const PrepParam
 			for (int i = numExtraImpulses - 1; i >= 0; --i)
 			{
 				endDecelSegs = MoveSegment::Allocate(endDecelSegs);
-				const float deceleration = dda.deceleration * (1.0 - coefficients[i]);
+				const float deceleration = params.shaped.deceleration * (1.0 - coefficients[i]);
 				const float segTime = durations[i];
 				segStartSpeed += deceleration * segTime;
 				const float b = segStartSpeed/deceleration;
@@ -791,7 +805,7 @@ MoveSegment *AxisShaper::GetDecelerationSegments(const DDA& dda, const PrepParam
 			accumulatedSegTime += totalShapingClocks;
 		}
 
-		float startDistance = params.decelStartDistance;
+		float startDistance = params.shaped.decelStartDistance;
 		float startSpeed = dda.topSpeed;
 		MoveSegment *startDecelSegs = nullptr;
 		if (params.shapingPlan.shapeDecelStart)
@@ -800,7 +814,7 @@ MoveSegment *AxisShaper::GetDecelerationSegments(const DDA& dda, const PrepParam
 			for (unsigned int i = 0; i < numExtraImpulses; ++i)
 			{
 				MoveSegment *seg = MoveSegment::Allocate(nullptr);
-				const float deceleration = dda.deceleration * coefficients[i];
+				const float deceleration = params.shaped.deceleration * coefficients[i];
 				const float segTime = durations[i];
 				const float b = startSpeed/deceleration;
 				const float c = -2.0/deceleration;
@@ -824,9 +838,9 @@ MoveSegment *AxisShaper::GetDecelerationSegments(const DDA& dda, const PrepParam
 		if (endDistance > startDistance)
 		{
 			endDecelSegs = MoveSegment::Allocate(endDecelSegs);
-			const float b = startSpeed/dda.deceleration;
-			const float c = -2.0/dda.deceleration;
-			endDecelSegs->SetNonLinear(endDistance - startDistance, params.decelClocks - accumulatedSegTime, b, c);
+			const float b = startSpeed/params.shaped.deceleration;
+			const float c = -2.0/params.shaped.deceleration;
+			endDecelSegs->SetNonLinear(endDistance - startDistance, params.shaped.decelClocks - accumulatedSegTime, b, c);
 		}
 
 		if (startDecelSegs == nullptr)
@@ -846,14 +860,14 @@ MoveSegment *AxisShaper::GetDecelerationSegments(const DDA& dda, const PrepParam
 
 // Generate the steady speed segment (if any), tack the segments together, and attach them to the DDA
 // Must set up params.steadyClocks before calling this
-MoveSegment *AxisShaper::FinishSegments(const DDA& dda, const PrepParams& params, MoveSegment *accelSegs, MoveSegment *decelSegs) const noexcept
+MoveSegment *AxisShaper::FinishShapedSegments(const DDA& dda, const PrepParams& params, MoveSegment *accelSegs, MoveSegment *decelSegs) const noexcept
 {
-	if (params.steadyClocks > 0.0)
+	if (params.shaped.steadyClocks > 0.0)
 	{
 		// Insert a steady speed segment before the deceleration segments
 		decelSegs = MoveSegment::Allocate(decelSegs);
 		const float c = 1.0/dda.topSpeed;
-		decelSegs->SetLinear(params.decelStartDistance - params.accelDistance, params.steadyClocks, c);
+		decelSegs->SetLinear(params.shaped.decelStartDistance - params.shaped.accelDistance, params.shaped.steadyClocks, c);
 	}
 
 	if (accelSegs != nullptr)
@@ -869,38 +883,38 @@ MoveSegment *AxisShaper::FinishSegments(const DDA& dda, const PrepParams& params
 }
 
 // Calculate the additional acceleration distance needed if we shape the start of acceleration
-inline float AxisShaper::GetExtraAccelStartDistance(const DDA& dda) const noexcept
+inline float AxisShaper::GetExtraAccelStartDistance(float startSpeed, float acceleration) const noexcept
 {
-	return (extraClocksAtStart * dda.startSpeed) + (extraDistanceAtStart * dda.acceleration);
+	return (extraClocksAtStart * startSpeed) + (extraDistanceAtStart * acceleration);
 }
 
 // Calculate the additional acceleration distance needed if we shape the end of acceleration
-inline float AxisShaper::GetExtraAccelEndDistance(const DDA& dda) const noexcept
+inline float AxisShaper::GetExtraAccelEndDistance(float topSpeed, float acceleration) const noexcept
 {
-	return (extraClocksAtEnd * dda.topSpeed) + (extraDistanceAtEnd * dda.acceleration);
+	return (extraClocksAtEnd * topSpeed) + (extraDistanceAtEnd * acceleration);
 }
 
-inline float AxisShaper::GetExtraDecelStartDistance(const DDA& dda) const noexcept
+inline float AxisShaper::GetExtraDecelStartDistance(float topSpeed, float deceleration) const noexcept
 {
-	return (extraClocksAtStart * dda.topSpeed) - (extraDistanceAtStart * dda.deceleration);
+	return (extraClocksAtStart * topSpeed) - (extraDistanceAtStart * deceleration);
 }
 
 // Calculate the additional deceleration distance needed if we shape the end of deceleration
-inline float AxisShaper::GetExtraDecelEndDistance(const DDA& dda) const noexcept
+inline float AxisShaper::GetExtraDecelEndDistance(float endSpeed, float deceleration) const noexcept
 {
-	return (extraClocksAtEnd * dda.endSpeed) - (extraDistanceAtEnd * dda.deceleration);
+	return (extraClocksAtEnd * endSpeed) - (extraDistanceAtEnd * deceleration);
 }
 
 /*static*/ MoveSegment *AxisShaper::GetUnshapedSegments(DDA& dda, const PrepParams& params) noexcept
 {
 	// Deceleration phase
 	MoveSegment * tempSegments;
-	if (params.decelClocks > 0.0)
+	if (params.unshaped.decelClocks > 0.0)
 	{
 		tempSegments = MoveSegment::Allocate(nullptr);
-		const float b = dda.topSpeed/dda.deceleration;
-		const float c = -2.0/dda.deceleration;
-		tempSegments->SetNonLinear(params.decelDistance, params.decelClocks, b, c);
+		const float b = dda.topSpeed/params.unshaped.deceleration;
+		const float c = -2.0/params.unshaped.deceleration;
+		tempSegments->SetNonLinear(params.unshaped.decelDistance, params.unshaped.decelClocks, b, c);
 	}
 	else
 	{
@@ -908,20 +922,20 @@ inline float AxisShaper::GetExtraDecelEndDistance(const DDA& dda) const noexcept
 	}
 
 	// Steady speed phase
-	if (params.steadyClocks > 0.0)
+	if (params.unshaped.steadyClocks > 0.0)
 	{
 		tempSegments = MoveSegment::Allocate(tempSegments);
 		const float c = 1.0/dda.topSpeed;
-		tempSegments->SetLinear(params.decelStartDistance - params.accelDistance, params.steadyClocks, c);
+		tempSegments->SetLinear(params.unshaped.decelStartDistance - params.unshaped.accelDistance, params.unshaped.steadyClocks, c);
 	}
 
 	// Acceleration phase
-	if (params.accelClocks > 0.0)
+	if (params.unshaped.accelClocks > 0.0)
 	{
 		tempSegments = MoveSegment::Allocate(tempSegments);
-		const float b = dda.startSpeed/(-dda.acceleration);
-		const float c = 2.0/dda.acceleration;
-		tempSegments->SetNonLinear(params.accelDistance, params.accelClocks, b, c);
+		const float b = dda.startSpeed/(-params.unshaped.acceleration);
+		const float c = 2.0/params.unshaped.acceleration;
+		tempSegments->SetNonLinear(params.unshaped.accelDistance, params.unshaped.accelClocks, b, c);
 	}
 
 	return tempSegments;
diff --git a/src/Movement/AxisShaper.h b/src/Movement/AxisShaper.h
index 752ca36c..7c6624ef 100644
--- a/src/Movement/AxisShaper.h
+++ b/src/Movement/AxisShaper.h
@@ -52,17 +52,17 @@ protected:
 private:
 	MoveSegment *GetAccelerationSegments(const DDA& dda, const PrepParams& params) const noexcept;
 	MoveSegment *GetDecelerationSegments(const DDA& dda, const PrepParams& params) const noexcept;
-	MoveSegment *FinishSegments(const DDA& dda, const PrepParams& params, MoveSegment *accelSegs, MoveSegment *decelSegs) const noexcept;
-	float GetExtraAccelStartDistance(const DDA& dda) const noexcept;
-	float GetExtraAccelEndDistance(const DDA& dda) const noexcept;
-	float GetExtraDecelStartDistance(const DDA& dda) const noexcept;
-	float GetExtraDecelEndDistance(const DDA& dda) const noexcept;
-	void TryShapeAccelStart(const DDA& dda, PrepParams& params) const noexcept;
+	MoveSegment *FinishShapedSegments(const DDA& dda, const PrepParams& params, MoveSegment *accelSegs, MoveSegment *decelSegs) const noexcept;
+	float GetExtraAccelStartDistance(float startSpeed, float acceleration) const noexcept;
+	float GetExtraAccelEndDistance(float topSpeed, float acceleration) const noexcept;
+	float GetExtraDecelStartDistance(float topSpeed, float deceleration) const noexcept;
+	float GetExtraDecelEndDistance(float endSpeed, float deceleration) const noexcept;
 	void TryShapeAccelEnd(const DDA& dda, PrepParams& params) const noexcept;
 	void TryShapeAccelBoth(DDA& dda, PrepParams& params) const noexcept;
 	void TryShapeDecelStart(const DDA& dda, PrepParams& params) const noexcept;
-	void TryShapeDecelEnd(const DDA& dda, PrepParams& params) const noexcept;
 	void TryShapeDecelBoth(DDA& dda, PrepParams& params) const noexcept;
+	bool ImplementAccelShaping(const DDA& dda, PrepParams& params, float newAccelDistance, float newAccelClocks) const noexcept;
+	bool ImplementDecelShaping(const DDA& dda, PrepParams& params, float newDecelDistance, float newDecelClocks) const noexcept;
 
 	static constexpr unsigned int MaxExtraImpulses = 4;
 	static constexpr float DefaultFrequency = 40.0;
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index 0f6a3c09..85762f53 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -118,13 +118,15 @@ void DDA::LogProbePosition() noexcept
 // Set up the parameters from the DDA, excluding steadyClocks because that may be affected by input shaping
 void PrepParams::SetFromDDA(const DDA& dda) noexcept
 {
-	decelDistance = dda.beforePrepare.decelDistance;
-	decelStartDistance = dda.totalDistance - dda.beforePrepare.decelDistance;
+	unshaped.decelDistance = dda.beforePrepare.decelDistance;
+	unshaped.decelStartDistance = dda.totalDistance - dda.beforePrepare.decelDistance;
 	// Due to rounding error, for an accelerate-decelerate move we may have accelDistance+decelDistance slightly greater than totalDistance.
 	// We need to make sure that accelDistance <= decelStartDistance for subsequent calculations to work.
-	accelDistance = min<float>(dda.beforePrepare.accelDistance, decelStartDistance);
-	accelClocks = (dda.topSpeed - dda.startSpeed)/dda.acceleration;
-	decelClocks = (dda.topSpeed - dda.endSpeed)/dda.deceleration;
+	unshaped.accelDistance = min<float>(dda.beforePrepare.accelDistance, unshaped.decelStartDistance);
+	unshaped.acceleration = dda.acceleration;
+	unshaped.deceleration = dda.deceleration;
+	unshaped.accelClocks = (dda.topSpeed - dda.startSpeed)/dda.acceleration;
+	unshaped.decelClocks = (dda.topSpeed - dda.endSpeed)/dda.deceleration;
 
 #if SUPPORT_CAN_EXPANSION
 	initialSpeedFraction = dda.startSpeed/dda.topSpeed;
@@ -133,11 +135,10 @@ void PrepParams::SetFromDDA(const DDA& dda) noexcept
 }
 
 // Calculate the steady clocks and set the total clocks in the DDA
-void PrepParams::Finalise(DDA& dda) noexcept
+void PrepParams::PrepParamSet::Finalise(float topSpeed) noexcept
 {
 	const float steadyDistance = decelStartDistance - accelDistance;
-	steadyClocks = (steadyDistance <= 0.0) ? 0.0 : steadyDistance/dda.topSpeed;
-	dda.clocksNeeded = (uint32_t)(accelClocks + decelClocks + steadyClocks);
+	steadyClocks = (steadyDistance <= 0.0) ? 0.0 : steadyDistance/topSpeed;
 }
 
 DDA::DDA(DDA* n) noexcept : next(n), prev(nullptr), state(empty)
@@ -264,11 +265,10 @@ void DDA::DebugPrint(const char *tag) const noexcept
 		DebugPrintVector(" end", endCoordinates, numAxes);
 	}
 
-	debugPrintf(" s=%f", (double)totalDistance);
+	debugPrintf(" s=%.4e", (double)totalDistance);
 	DebugPrintVector(" vec", directionVector, MaxAxesPlusExtruders);
-	debugPrintf("\n"
-				"a=%.3e d=%.3e reqv=%.3e startv=%.3e topv=%.3e endv=%.3e cks=%" PRIu32 "\n",
-				(double)acceleration, (double)deceleration, (double)requestedSpeed, (double)startSpeed, (double)topSpeed, (double)endSpeed, clocksNeeded);
+	debugPrintf("\n" "a=%.4e d=%.4e reqv=%.4e startv=%.4e topv=%.4e endv=%.4e cks=%" PRIu32 " fp=%" PRIu32 " fl=%04x\n",
+				(double)acceleration, (double)deceleration, (double)requestedSpeed, (double)startSpeed, (double)topSpeed, (double)endSpeed, clocksNeeded, (uint32_t)filePos, flags.all);
 	for (const MoveSegment *segs = shapedSegments; segs != nullptr; segs = segs->GetNext())
 	{
 		segs->DebugPrint('S');
@@ -708,12 +708,12 @@ bool DDA::InitFromRemote(const CanMessageMovementLinear& msg) noexcept
 	deceleration = (msg.decelClocks == 0) ? 0.0 : (topSpeed * (1.0 - msg.finalSpeedFraction))/msg.decelClocks;
 
 	PrepParams params;
-	params.accelDistance = topSpeed * (1.0 + msg.initialSpeedFraction) * msg.accelerationClocks * 0.5;
-	params.decelDistance = topSpeed * (1.0 + msg.finalSpeedFraction) * msg.decelClocks * 0.5;
-	params.decelStartDistance = 1.0 - params.decelDistance;
-	params.accelClocks = msg.accelerationClocks;
-	params.steadyClocks = msg.steadyClocks;
-	params.decelClocks = msg.decelClocks;
+	params.unshaped.accelDistance = topSpeed * (1.0 + msg.initialSpeedFraction) * msg.accelerationClocks * 0.5;
+	params.unshaped.decelDistance = topSpeed * (1.0 + msg.finalSpeedFraction) * msg.decelClocks * 0.5;
+	params.unshaped.decelStartDistance = 1.0 - params.unshaped.decelDistance;
+	params.unshaped.accelClocks = msg.accelerationClocks;
+	params.unshaped.steadyClocks = msg.steadyClocks;
+	params.unshaped.decelClocks = msg.decelClocks;
 
 	shapedSegments = unshapedSegments = nullptr;
 	activeDMs = completedDMs = nullptr;
@@ -1297,14 +1297,20 @@ void DDA::Prepare(uint8_t simMode) noexcept
 	PrepParams params;										// the default constructor clears params.plan to 'no shaping'
 	if (flags.xyMoving)
 	{
-		reprap.GetMove().GetAxisShaper().PlanShaping(*this, params, flags.xyMoving);		// this will set up shapedSegments if we are doing any shaping
+		reprap.GetMove().GetAxisShaper().PlanShaping(*this, params, flags.xyMoving);	// this will set up shapedSegments if we are doing any shaping
 	}
 	else
 	{
 		params.SetFromDDA(*this);
-		params.Finalise(*this);
+		params.unshaped.Finalise(topSpeed);
+		clocksNeeded = params.unshaped.TotalClocks();
 	}
 
+	// Copy the unshaped acceleration and deceleration back to the DDA because ManageLaserPower uses them
+	//TODO change ManageLaserPower to work on the shaped segments instead
+	acceleration = params.unshaped.acceleration;
+	deceleration = params.unshaped.deceleration;
+
 	if (simMode == 0)
 	{
 		if (flags.isDeltaMovement)
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index d9c06a45..a388c092 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -27,12 +27,24 @@ class DDARing;
 // Struct for passing parameters to the DriveMovement Prepare methods, also accessed by the input shaper
 struct PrepParams
 {
-	// Parameters used for all types of motion
-	float accelDistance;
-	float decelDistance;
-	float decelStartDistance;
-	float accelClocks, steadyClocks, decelClocks;
+	struct PrepParamSet
+	{
+		float accelDistance;
+		float decelDistance;
+		float decelStartDistance;
+		float accelClocks, steadyClocks, decelClocks;
+		float acceleration, deceleration;
 
+		// Calculate the steady clocks and set the total clocks in the DDA
+		void Finalise(float topSpeed) noexcept;
+
+		// Get the total clocks needed
+		float TotalClocks() const noexcept { return accelClocks + steadyClocks + decelClocks; }
+	};
+
+	// Parameters used for all types of motion
+	PrepParamSet unshaped;
+	PrepParamSet shaped;								// only valid if the shaping plan is not empty
 	InputShaperPlan shapingPlan;
 
 #if SUPPORT_CAN_EXPANSION
@@ -51,9 +63,6 @@ struct PrepParams
 
 	// Set up the parameters from the DDA, excluding steadyClocks because that may be affected by input shaping
 	void SetFromDDA(const DDA& dda) noexcept;
-
-	// Calculate the steady clocks and set the total clocks in the DDA
-	void Finalise(DDA& dda) noexcept;
 };
 
 // This defines a single coordinated movement of one or several motors
diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp
index a73817f1..fca69228 100644
--- a/src/Movement/DriveMovement.cpp
+++ b/src/Movement/DriveMovement.cpp
@@ -58,16 +58,16 @@ void DriveMovement::DebugPrint() const noexcept
 	const char c = (drive < reprap.GetGCodes().GetTotalAxes()) ? reprap.GetGCodes().GetAxisLetters()[drive] : (char)('0' + LogicalDriveToExtruder(drive));
 	if (state != DMState::idle)
 	{
-		debugPrintf("DM%c%s dir=%c steps=%" PRIu32 " next=%" PRIu32 " rev=%" PRIu32 " interval=%" PRIu32 " psl=%" PRIu32 " A=%.3e B=%.3e C=%.3e",
+		debugPrintf("DM%c%s dir=%c steps=%" PRIu32 " next=%" PRIu32 " rev=%" PRIu32 " interval=%" PRIu32 " psl=%" PRIu32 " A=%.4e B=%.4e C=%.4e",
 						c, (state == DMState::stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, reverseStartStep, stepInterval, phaseStepLimit, (double)pA, (double)pB, (double)pC);
 		if (isDelta)
 		{
-			debugPrintf(" hmz0s=%.2f minusAaPlusBbTimesS=%.2f dSquaredMinusAsquaredMinusBsquared=%.2f drev=%.3f\n",
+			debugPrintf(" hmz0s=%.4e minusAaPlusBbTimesS=%.4e dSquaredMinusAsquaredMinusBsquared=%.4e drev=%.4e\n",
 							(double)mp.delta.fHmz0s, (double)mp.delta.fMinusAaPlusBbTimesS, (double)mp.delta.fDSquaredMinusAsquaredMinusBsquaredTimesSsquared, (double)mp.delta.reverseStartDistance);
 		}
 		else if (isExtruder)
 		{
-			debugPrintf(" pa=%" PRIu32 " eed=%.3f\n", (uint32_t)mp.cart.pressureAdvanceK, (double)mp.cart.extraExtrusionDistance);
+			debugPrintf(" pa=%" PRIu32 " eed=%.4e ebf=%.4e\n", (uint32_t)mp.cart.pressureAdvanceK, (double)mp.cart.extraExtrusionDistance, (double)mp.cart.extrusionBroughtForwards);
 		}
 		else
 		{
@@ -367,13 +367,13 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n
 // We have already generated the extruder segments and we know that there are some
 bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) noexcept
 {
-	ExtruderShaper& shaper = reprap.GetMove().GetExtruderShaper(LogicalDriveToExtruder(drive));
-	distanceSoFar = shaper.GetExtrusionPending()/dda.directionVector[drive];
-
 	const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive);
 	mp.cart.effectiveStepsPerMm = stepsPerMm * fabsf(dda.directionVector[drive]);
 	mp.cart.effectiveMmPerStep = 1.0/mp.cart.effectiveStepsPerMm;
 
+	ExtruderShaper& shaper = reprap.GetMove().GetExtruderShaper(LogicalDriveToExtruder(drive));
+	mp.cart.extrusionBroughtForwards = distanceSoFar = shaper.GetExtrusionPending()/dda.directionVector[drive];
+
 	// Calculate the total forward and reverse movement distances
 	float forwardDistance = distanceSoFar;
 	float reverseDistance;
@@ -382,7 +382,7 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no
 	{
 		// We are using nonzero pressure advance. Movement must be forwards.
 		mp.cart.pressureAdvanceK = shaper.GetKclocks();
-		mp.cart.extraExtrusionDistance = mp.cart.pressureAdvanceK * dda.acceleration * params.accelClocks;
+		mp.cart.extraExtrusionDistance = mp.cart.pressureAdvanceK * params.unshaped.acceleration * params.unshaped.accelClocks;
 		forwardDistance += mp.cart.extraExtrusionDistance;
 
 		// Check if there is a reversal in the deceleration segment
@@ -400,27 +400,27 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no
 		}
 		else
 		{
-			const float initialDecelSpeed = dda.topSpeed - mp.cart.pressureAdvanceK * dda.deceleration;
+			const float initialDecelSpeed = dda.topSpeed - mp.cart.pressureAdvanceK * params.unshaped.deceleration;
 			if (initialDecelSpeed <= 0.0)
 			{
 				// The entire deceleration segment is in reverse
-				forwardDistance += params.decelStartDistance;
-				reverseDistance = ((0.5 * dda.deceleration * params.decelClocks) - initialDecelSpeed) * params.decelClocks;
+				forwardDistance += params.unshaped.decelStartDistance;
+				reverseDistance = ((0.5 * params.unshaped.deceleration * params.unshaped.decelClocks) - initialDecelSpeed) * params.unshaped.decelClocks;
 			}
 			else
 			{
 				const float timeToReverse = initialDecelSpeed * ((-0.5) * decelSeg->GetC());	// 'c' is -2/deceleration, so -0.5*c is 1/deceleration
-				if (timeToReverse < params.decelClocks)
+				if (timeToReverse < params.unshaped.decelClocks)
 				{
 					// There is a reversal
-					const float distanceToReverse = 0.5 * dda.deceleration * fsquare(timeToReverse);
-					forwardDistance += params.decelStartDistance + distanceToReverse;
-					reverseDistance = 0.5 * dda.deceleration * fsquare(params.decelClocks - timeToReverse);
+					const float distanceToReverse = 0.5 * params.unshaped.deceleration * fsquare(timeToReverse);
+					forwardDistance += params.unshaped.decelStartDistance + distanceToReverse;
+					reverseDistance = 0.5 * params.unshaped.deceleration * fsquare(params.unshaped.decelClocks - timeToReverse);
 				}
 				else
 				{
 					// No reversal
-					forwardDistance += dda.totalDistance - (mp.cart.pressureAdvanceK * dda.deceleration * params.decelClocks);
+					forwardDistance += dda.totalDistance - (mp.cart.pressureAdvanceK * params.unshaped.deceleration * params.unshaped.decelClocks);
 					reverseDistance = 0.0;
 				}
 			}
@@ -629,36 +629,36 @@ pre(nextStep <= totalSteps; stepsTillRecalc == 0)
 		return false;
 	}
 
-	// When crossing between movement phases with high microstepping, due to rounding errors the next step may appear to be due before the last one
-	stepInterval = (nextCalcStepTime > nextStepTime)
-					? (nextCalcStepTime - nextStepTime) >> shiftFactor	// calculate the time per step, ready for next time
-					: 0;
-#if EVEN_STEPS
-	nextStepTime = nextCalcStepTime - (stepsTillRecalc * stepInterval);
-#else
-	nextStepTime = nextCalcStepTime;
-#endif
-
 	if (nextCalcStepTime > dda.clocksNeeded)
 	{
 		// The calculation makes this step late.
 		// When the end speed is very low, calculating the time of the last step is very sensitive to rounding error.
 		// So if this is the last step and it is late, bring it forward to the expected finish time.
 		// Very rarely on a delta, the penultimate step may also be calculated late. Allow for that here in case it affects Cartesian axes too.
-		if (nextStep + 1 >= totalSteps)
+		if (nextStep + stepsTillRecalc + 1 >= totalSteps)
 		{
-			nextStepTime = dda.clocksNeeded;
+			nextCalcStepTime = dda.clocksNeeded;
 		}
 		else
 		{
 			// We don't expect any step except the last to be late
 			state = DMState::stepError;
-			nextStep += 120000000;							// so we can tell what happened in the debug print
-			stepInterval = nextCalcStepTime;	//DEBUG
+			nextStep += 120000000 + stepsTillRecalc;		// so we can tell what happened in the debug print
+			stepInterval = nextCalcStepTime;				//DEBUG
 			return false;
 		}
 	}
 
+	// When crossing between movement phases with high microstepping, due to rounding errors the next step may appear to be due before the last one
+	stepInterval = (nextCalcStepTime > nextStepTime)
+					? (nextCalcStepTime - nextStepTime) >> shiftFactor	// calculate the time per step, ready for next time
+					: 0;
+#if EVEN_STEPS
+	nextStepTime = nextCalcStepTime - (stepsTillRecalc * stepInterval);
+#else
+	nextStepTime = nextCalcStepTime;
+#endif
+
 	return true;
 }
 
diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h
index 21e1d421..75ad8346 100644
--- a/src/Movement/DriveMovement.h
+++ b/src/Movement/DriveMovement.h
@@ -136,6 +136,7 @@ private:
 			float effectiveStepsPerMm;					// the steps/mm multiplied by the movement fraction
 			float effectiveMmPerStep;					// reciprocal of [the steps/mm multiplied by the movement fraction]
 			float extraExtrusionDistance;				// the extra extrusion distance in the acceleration phase
+			float extrusionBroughtForwards;				// the amount of extrusion brought forwards from previous moves. Only needed for debug output.
 		} cart;
 	} mp;
 
diff --git a/src/Movement/MoveSegment.cpp b/src/Movement/MoveSegment.cpp
index 3f9e9763..f2f8c244 100644
--- a/src/Movement/MoveSegment.cpp
+++ b/src/Movement/MoveSegment.cpp
@@ -50,7 +50,7 @@ void MoveSegment::AddToTail(MoveSegment *tail) noexcept
 
 void MoveSegment::DebugPrint(char ch) const noexcept
 {
-	debugPrintf("%c d=%7.3f t=%7" PRIu32 " ", ch, (double)segmentLength, (uint32_t)segTime);
+	debugPrintf("%c d=%.3e t=%" PRIu32 " ", ch, (double)segmentLength, (uint32_t)segTime);
 	if (IsLinear())
 	{
 		debugPrintf("c=%.3e\n", (double)c);