Fixed bug in new Cartesian movement code

5 files changed, 53 insertions, 16 deletions

diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index d4556922..038789d6 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -310,6 +310,9 @@ bool DDA::InitStandardMove(DDARing& ring, const RawMove &nextMove, bool doMotorM
 
 			if (delta != 0)
 			{
+#if 0	// debug only
+				stepsRequested[drive] += labs(delta);
+#endif
 				if (reprap.GetPlatform().IsAxisRotational(drive))
 				{
 					rotationalAxesMoving = true;
@@ -1968,6 +1971,11 @@ pre(state == frozen)
 uint32_t DDA::lastStepLowTime = 0;
 uint32_t DDA::lastDirChangeTime = 0;
 
+#if 0	// debug only
+uint32_t DDA::stepsRequested[NumDirectDrivers];
+uint32_t DDA::stepsDone[NumDirectDrivers];
+#endif
+
 // Generate the step pulses of internal drivers used by this DDA
 // Sets the status to 'completed' if the move is complete and the next move should be started
 void DDA::StepDrivers(Platform& p) noexcept
@@ -1989,6 +1997,9 @@ void DDA::StepDrivers(Platform& p) noexcept
 	while (dm != nullptr && elapsedTime >= dm->nextStepTime)		// if the next step is due
 	{
 		driversStepping |= p.GetDriversBitmap(dm->drive);
+#if 0	// debug only
+		++stepsDone[dm->drive];
+#endif
 		dm = dm->nextDM;
 	}
 
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index c198a848..b22eea44 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -178,6 +178,10 @@ public:
 	static uint32_t lastStepLowTime;										// when we last completed a step pulse to a slow driver
 	static uint32_t lastDirChangeTime;										// when we last change the DIR signal to a slow driver
 
+#if 0	// debug only
+	static uint32_t stepsRequested[NumDirectDrivers], stepsDone[NumDirectDrivers];
+#endif
+
 private:
 	DriveMovement *FindDM(size_t drive) const noexcept;						// find the DM for a drive if there is one even if it is completed
 	DriveMovement *FindActiveDM(size_t drive) const noexcept;				// find the DM for a drive if there is one but only if it is active
diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp
index 66f2133b..65b2bc9d 100644
--- a/src/Movement/DriveMovement.cpp
+++ b/src/Movement/DriveMovement.cpp
@@ -394,7 +394,8 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params, fl
 	stepsTillRecalc = 0;							// so that we don't skip the calculation
 	state = (mp.cart.accelStopStep > 1) ? DMState::accel0
 				: (mp.cart.decelStartStep > 1) ? DMState::steady
-				  : DMState::decel0;
+					: (reverseStartStep > 1) ? DMState::decel0
+						: DMState::reversing;
 	isDelta = false;
 	return CalcNextStepTime(dda);
 }
@@ -503,6 +504,10 @@ bool DriveMovement::PrepareRemoteExtruder(const DDA& dda, const PrepParams& para
 	nextStepTime = 0;
 	stepInterval = 999999;							// initialise to a large value so that we will calculate the time for just one step
 	stepsTillRecalc = 0;							// so that we don't skip the calculation
+	state = (mp.cart.accelStopStep > 1) ? DMState::accel0
+				: (mp.cart.decelStartStep > 1) ? DMState::steady
+					: (reverseStartStep > 1) ? DMState::decel0
+						: DMState::reversing;
 	isDelta = false;
 	return CalcNextStepTime(dda);
 }
@@ -575,15 +580,15 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 	uint32_t nextCalcStepTime;
 	switch (state)
 	{
-	case DMState::accel0:
-		// acceleration phase
+	case DMState::accel0:	// acceleration phase
 		{
 			const uint32_t stepsToLimit = mp.cart.accelStopStep - nextStep;
 			if (stepsToLimit == 1)
 			{
+				// This is the last step in this phase
 				state = (mp.cart.decelStartStep > mp.cart.accelStopStep) ? DMState::steady
 						: (reverseStartStep > mp.cart.accelStopStep) ? DMState::decel0
-							: DMState::reverse;
+							: DMState::reversing;
 			}
 			else if (stepInterval < DDA::MinCalcIntervalCartesian)
 			{
@@ -613,14 +618,13 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 		}
 		break;
 
-	case DMState::steady:
-		// steady speed phase
+	case DMState::steady:	// steady speed phase
 		{
 			const uint32_t stepsToLimit = mp.cart.decelStartStep - nextStep;
 			if (stepsToLimit == 1)
 			{
 				state = (reverseStartStep > mp.cart.decelStartStep) ? DMState::decel0
-							: DMState::reverse;
+							: DMState::reversing;
 			}
 			else if (stepInterval < DDA::MinCalcIntervalCartesian)
 			{
@@ -655,15 +659,12 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 		}
 		break;
 
-	case DMState::decel0:
-		// deceleration phase, not reversed yet
+	case DMState::decel0:	// deceleration phase, not reversed yet
 		{
 			const uint32_t stepsToLimit = reverseStartStep - nextStep;
 			if (stepsToLimit == 1)
 			{
-				direction = !direction;
-				directionChanged = true;
-				state = DMState::reverse;
+				state = DMState::reversing;
 			}
 			else if (stepInterval < DDA::MinCalcIntervalCartesian)
 			{
@@ -700,7 +701,12 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 		}
 		break;
 
-	case DMState::reverse:
+	case DMState::reversing:
+		direction = !direction;
+		directionChanged = true;
+		state = DMState::reverse;
+		// no break
+	case DMState::reverse:	// reverse phase
 		{
 			const uint32_t stepsToLimit = totalSteps + 1 - nextStep;
 			if (stepInterval < DDA::MinCalcIntervalCartesian)
diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h
index 8ce442aa..eb09efee 100644
--- a/src/Movement/DriveMovement.h
+++ b/src/Movement/DriveMovement.h
@@ -147,6 +147,7 @@ enum class DMState : uint8_t
 	decel5,
 	decel6,
 	decel7,
+	reversing,
 	reverse
 };
 
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 2e39943e..aee6946d 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -417,14 +417,29 @@ bool Move::LowPowerOrStallPause(RestorePoint& rp) noexcept
 void Move::Diagnostics(MessageType mtype) noexcept
 {
 	// Get the type of bed compensation in use
-	String<StringLength50> bedCompString;
-	bedCompString.copy(GetCompensationTypeString());
+#if 0	// debug only
+	String<StringLength256> scratchString;
+#else
+	String<StringLength50> scratchString;
+#endif
+	scratchString.copy(GetCompensationTypeString());
 
 	Platform& p = reprap.GetPlatform();
 	p.MessageF(mtype, "=== Move ===\nDMs created %u, maxWait %" PRIu32 "ms, bed compensation in use: %s, comp offset %.3f\n",
-						DriveMovement::NumCreated(), longestGcodeWaitInterval, bedCompString.c_str(), (double)zShift);
+						DriveMovement::NumCreated(), longestGcodeWaitInterval, scratchString.c_str(), (double)zShift);
 	longestGcodeWaitInterval = 0;
 
+#if 0	// debug only
+	scratchString.copy("Steps requested/done:");
+	for (size_t driver = 0; driver < NumDirectDrivers; ++driver)
+	{
+		scratchString.catf(" %" PRIu32 "/%" PRIu32, DDA::stepsRequested[driver], DDA::stepsDone[driver]);
+		DDA::stepsRequested[driver] = DDA::stepsDone[driver] = 0;
+	}
+	scratchString.cat('\n');
+	p.Message(mtype, scratchString.c_str());
+#endif
+
 #if DDA_LOG_PROBE_CHANGES
 	// Temporary code to print Z probe trigger positions
 	p.Message(mtype, "Probe change coordinates:");