Version 1.20alpha5

Check arm angle limits when computing SCARA moves
Don't allow SCARA arm mode changes on coordinated moves

19 files changed, 48 insertions, 28 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index a576a26d..8b4f4502 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -504,6 +504,7 @@ void GCodes::Spin()
 				}
 				moveBuffer.feedRate = DefaultFeedrate * SecondsToMinutes;	// ask for a good feed rate, we may have paused during a slow move
 				moveBuffer.moveType = 0;
+				moveBuffer.isCoordinated = false;
 				moveBuffer.endStopsToCheck = 0;
 				moveBuffer.usePressureAdvance = false;
 				moveBuffer.filePos = noFilePosition;
@@ -616,6 +617,7 @@ void GCodes::Spin()
 					if (move.IsAccessibleProbePoint(x, y))
 					{
 						moveBuffer.moveType = 0;
+						moveBuffer.isCoordinated = false;
 						moveBuffer.endStopsToCheck = 0;
 						moveBuffer.usePressureAdvance = false;
 						moveBuffer.filePos = noFilePosition;
@@ -677,6 +679,7 @@ void GCodes::Spin()
 					zProbeTriggered = false;
 					platform.SetProbing(true);
 					moveBuffer.moveType = 0;
+					moveBuffer.isCoordinated = false;
 					moveBuffer.endStopsToCheck = ZProbeActive;
 					moveBuffer.usePressureAdvance = false;
 					moveBuffer.filePos = noFilePosition;
@@ -720,6 +723,7 @@ void GCodes::Spin()
 
 				// Move back up to the dive height
 				moveBuffer.moveType = 0;
+				moveBuffer.isCoordinated = false;
 				moveBuffer.endStopsToCheck = 0;
 				moveBuffer.usePressureAdvance = false;
 				moveBuffer.filePos = noFilePosition;
@@ -801,6 +805,7 @@ void GCodes::Spin()
 		case GCodeState::probingAtPoint0:
 			// Initial state when executing G30 with a P parameter. Start by moving to the dive height at the current position.
 			moveBuffer.moveType = 0;
+			moveBuffer.isCoordinated = false;
 			moveBuffer.endStopsToCheck = 0;
 			moveBuffer.usePressureAdvance = false;
 			moveBuffer.filePos = noFilePosition;
@@ -819,6 +824,7 @@ void GCodes::Spin()
 				// Head is at the dive height but needs to be moved to the correct XY position.
 				// The XY coordinates have already been stored.
 				moveBuffer.moveType = 0;
+				moveBuffer.isCoordinated = false;
 				moveBuffer.endStopsToCheck = 0;
 				moveBuffer.usePressureAdvance = false;
 				moveBuffer.filePos = noFilePosition;
@@ -877,6 +883,7 @@ void GCodes::Spin()
 					zProbeTriggered = false;
 					platform.SetProbing(true);
 					moveBuffer.moveType = 0;
+					moveBuffer.isCoordinated = false;
 					moveBuffer.endStopsToCheck = ZProbeActive;
 					moveBuffer.usePressureAdvance = false;
 					moveBuffer.filePos = noFilePosition;
@@ -971,6 +978,7 @@ void GCodes::Spin()
 
 					// Move back up to the dive height before we change anything, in particular before we adjust leadscrews
 					moveBuffer.moveType = 0;
+					moveBuffer.isCoordinated = false;
 					moveBuffer.endStopsToCheck = 0;
 					moveBuffer.usePressureAdvance = false;
 					moveBuffer.filePos = noFilePosition;
@@ -1040,6 +1048,7 @@ void GCodes::Spin()
 				moveBuffer.coords[Z_AXIS] += retractHop;
 				currentZHop = retractHop;
 				moveBuffer.moveType = 0;
+				moveBuffer.isCoordinated = false;
 				moveBuffer.isFirmwareRetraction = true;
 				moveBuffer.usePressureAdvance = false;
 				moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition;
@@ -1071,6 +1080,7 @@ void GCodes::Spin()
 					}
 					moveBuffer.feedRate = unRetractSpeed;
 					moveBuffer.moveType = 0;
+					moveBuffer.isCoordinated = false;
 					moveBuffer.isFirmwareRetraction = true;
 					moveBuffer.usePressureAdvance = false;
 					moveBuffer.filePos = (&gb == fileGCode) ? gb.MachineState().fileState.GetPosition() - fileInput->BytesCached() : noFilePosition;
@@ -1813,6 +1823,7 @@ bool GCodes::LoadExtrusionAndFeedrateFromGCode(GCodeBuffer& gb, int moveType)
 bool GCodes::DoStraightMove(GCodeBuffer& gb, StringRef& reply, bool isCoordinated)
 {
 	// Set up default move parameters
+	moveBuffer.isCoordinated = isCoordinated;
 	moveBuffer.endStopsToCheck = 0;
 	moveBuffer.moveType = 0;
 	doingArcMove = false;
@@ -2086,6 +2097,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise)
 	moveBuffer.virtualExtruderPosition = virtualExtruderPosition;
 	moveBuffer.endStopsToCheck = 0;
 	moveBuffer.usePressureAdvance = true;
+	moveBuffer.isCoordinated = true;
 	moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition;
 
 	// Set up the arc centre coordinates and record which axes behave like an X axis.
@@ -3599,6 +3611,7 @@ bool GCodes::RetractFilament(GCodeBuffer& gb, bool retract)
 			moveBuffer.coords[i] = 0.0;
 		}
 		moveBuffer.moveType = 0;
+		moveBuffer.isCoordinated = false;
 		moveBuffer.isFirmwareRetraction = true;
 		moveBuffer.usePressureAdvance = false;
 		moveBuffer.filePos = (&gb == fileGCode) ? gb.GetFilePosition(fileInput->BytesCached()) : noFilePosition;
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index e3806556..22bea4b9 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -88,11 +88,12 @@ public:
 		IoBits_t ioBits;												// I/O bits to set/clear at the start of this move
 #endif
 		uint8_t moveType;												// the S parameter from the G0 or G1 command, 0 for a normal move
-		bool isFirmwareRetraction;										// true if this is a firmware retraction/un-retraction move
-		bool usePressureAdvance;										// true if we want to us extruder pressure advance, if there is any extrusion
-		bool canPauseBefore;											// true if we can pause before this move
-		bool canPauseAfter;												// true if we can pause just after this move and successfully restart
-		bool hasExtrusion;												// true if the move includes extrusion - only valid if the move was set up by SetupMove
+		uint8_t isFirmwareRetraction : 1;								// true if this is a firmware retraction/un-retraction move
+		uint8_t usePressureAdvance : 1;									// true if we want to us extruder pressure advance, if there is any extrusion
+		uint8_t canPauseBefore : 1;										// true if we can pause before this move
+		uint8_t canPauseAfter : 1;										// true if we can pause just after this move and successfully restart
+		uint8_t hasExtrusion : 1;										// true if the move includes extrusion - only valid if the move was set up by SetupMove
+		uint8_t isCoordinated : 1;										// true if this is a coordinates move
 	};
   
 	GCodes(Platform& p);
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index 4a0bd8f7..ef92c444 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -225,7 +225,7 @@ bool DDA::Init(GCodes::RawMove &nextMove, bool doMotorMapping)
 	const Move& move = reprap.GetMove();
 	if (doMotorMapping)
 	{
-		if (!move.CartesianToMotorSteps(nextMove.coords, endPoint))		// transform the axis coordinates if on a delta or CoreXY printer
+		if (!move.CartesianToMotorSteps(nextMove.coords, endPoint, !nextMove.isCoordinated))		// transform the axis coordinates if on a delta or CoreXY printer
 		{
 			return false;												// throw away the move if it couldn't be transformed
 		}
diff --git a/src/Movement/Kinematics/CartesianKinematics.cpp b/src/Movement/Kinematics/CartesianKinematics.cpp
index dba9c2be..db884171 100644
--- a/src/Movement/Kinematics/CartesianKinematics.cpp
+++ b/src/Movement/Kinematics/CartesianKinematics.cpp
@@ -19,7 +19,7 @@ const char *CartesianKinematics::GetName(bool forStatusReport) const
 }
 
 // Convert Cartesian coordinates to motor coordinates
-bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const
+bool CartesianKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
 {
 	for (size_t axis = 0; axis < numVisibleAxes; ++axis)
 	{
diff --git a/src/Movement/Kinematics/CartesianKinematics.h b/src/Movement/Kinematics/CartesianKinematics.h
index eff75eb9..22051f6f 100644
--- a/src/Movement/Kinematics/CartesianKinematics.h
+++ b/src/Movement/Kinematics/CartesianKinematics.h
@@ -17,7 +17,7 @@ public:
 
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
-    bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override;
+    bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
     void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	HomingMode GetHomingMode() const override { return homeCartesianAxes; }
 	bool QueryTerminateHomingMove(size_t axis) const override;
diff --git a/src/Movement/Kinematics/CoreXYKinematics.cpp b/src/Movement/Kinematics/CoreXYKinematics.cpp
index c8b2bdaf..6f35e321 100644
--- a/src/Movement/Kinematics/CoreXYKinematics.cpp
+++ b/src/Movement/Kinematics/CoreXYKinematics.cpp
@@ -18,7 +18,7 @@ const char *CoreXYKinematics::GetName(bool forStatusReport) const
 }
 
 // Convert Cartesian coordinates to motor coordinates returning true if successful
-bool CoreXYKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const
+bool CoreXYKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
 {
 	motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]);
 	motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]);
diff --git a/src/Movement/Kinematics/CoreXYKinematics.h b/src/Movement/Kinematics/CoreXYKinematics.h
index 0b426c55..4663acc1 100644
--- a/src/Movement/Kinematics/CoreXYKinematics.h
+++ b/src/Movement/Kinematics/CoreXYKinematics.h
@@ -17,7 +17,7 @@ public:
 
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	bool DriveIsShared(size_t drive) const override;
 };
diff --git a/src/Movement/Kinematics/CoreXYUKinematics.cpp b/src/Movement/Kinematics/CoreXYUKinematics.cpp
index b961bebb..560037aa 100644
--- a/src/Movement/Kinematics/CoreXYUKinematics.cpp
+++ b/src/Movement/Kinematics/CoreXYUKinematics.cpp
@@ -55,7 +55,7 @@ bool CoreXYUKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, StringRef
 }
 
 // Convert Cartesian coordinates to motor coordinates
-bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const
+bool CoreXYUKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
 {
 	motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[X_AXIS]);
 	motorPos[Y_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) - (machinePos[Y_AXIS] * axisFactors[Y_AXIS])) * stepsPerMm[Y_AXIS]);
diff --git a/src/Movement/Kinematics/CoreXYUKinematics.h b/src/Movement/Kinematics/CoreXYUKinematics.h
index 67034f13..77db9ae9 100644
--- a/src/Movement/Kinematics/CoreXYUKinematics.h
+++ b/src/Movement/Kinematics/CoreXYUKinematics.h
@@ -18,7 +18,7 @@ public:
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
 	bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	bool DriveIsShared(size_t drive) const override;
 };
diff --git a/src/Movement/Kinematics/CoreXZKinematics.cpp b/src/Movement/Kinematics/CoreXZKinematics.cpp
index 1d29c8fc..d81f13ca 100644
--- a/src/Movement/Kinematics/CoreXZKinematics.cpp
+++ b/src/Movement/Kinematics/CoreXZKinematics.cpp
@@ -18,7 +18,7 @@ const char *CoreXZKinematics::GetName(bool forStatusReport) const
 }
 
 // Convert Cartesian coordinates to motor coordinates
-bool CoreXZKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const
+bool CoreXZKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
 {
 	motorPos[X_AXIS] = lrintf(((machinePos[X_AXIS] * axisFactors[X_AXIS]) + (machinePos[Z_AXIS] * axisFactors[Z_AXIS])) * stepsPerMm[X_AXIS]);
 	motorPos[Y_AXIS] = lrintf(machinePos[Y_AXIS] * stepsPerMm[Y_AXIS]);
diff --git a/src/Movement/Kinematics/CoreXZKinematics.h b/src/Movement/Kinematics/CoreXZKinematics.h
index 94b8a9f8..3a8d9344 100644
--- a/src/Movement/Kinematics/CoreXZKinematics.h
+++ b/src/Movement/Kinematics/CoreXZKinematics.h
@@ -17,7 +17,7 @@ public:
 
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	AxesBitmap AxesToHomeBeforeProbing() const override { return MakeBitmap<AxesBitmap>(X_AXIS) | MakeBitmap<AxesBitmap>(Y_AXIS) | MakeBitmap<AxesBitmap>(Z_AXIS); }
 	bool DriveIsShared(size_t drive) const override;
diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h
index 8dfe3da6..3d960929 100644
--- a/src/Movement/Kinematics/Kinematics.h
+++ b/src/Movement/Kinematics/Kinematics.h
@@ -71,7 +71,7 @@ public:
 	// 'numAxes' is the number of machine axes to convert, which will always be at least 3
 	// 'motorPos' is the output vector of motor positions
 	// Return true if successful, false if we were unable to convert
-	virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const = 0;
+	virtual bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const = 0;
 
 	// Convert motor positions (measured in steps from reference position) to Cartesian coordinates
 	// 'motorPos' is the input vector of motor positions
diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.cpp b/src/Movement/Kinematics/LinearDeltaKinematics.cpp
index 316c0ef6..8ff47f0e 100644
--- a/src/Movement/Kinematics/LinearDeltaKinematics.cpp
+++ b/src/Movement/Kinematics/LinearDeltaKinematics.cpp
@@ -134,7 +134,7 @@ void LinearDeltaKinematics::InverseTransform(float Ha, float Hb, float Hc, float
 }
 
 // Convert Cartesian coordinates to motor steps
-bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const
+bool LinearDeltaKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
 {
 	//TODO return false if we can't transform the position
 	for (size_t axis = 0; axis < min<size_t>(numVisibleAxes, DELTA_AXES); ++axis)
diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.h b/src/Movement/Kinematics/LinearDeltaKinematics.h
index 875bd126..6a3bdc3f 100644
--- a/src/Movement/Kinematics/LinearDeltaKinematics.h
+++ b/src/Movement/Kinematics/LinearDeltaKinematics.h
@@ -26,7 +26,7 @@ public:
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
 	bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	bool SupportsAutoCalibration() const override { return true; }
 	bool DoAutoCalibration(size_t numFactors, const RandomProbePointSet& probePoints, StringRef& reply) override;
diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp
index 58438d25..a350f42b 100644
--- a/src/Movement/Kinematics/ScaraKinematics.cpp
+++ b/src/Movement/Kinematics/ScaraKinematics.cpp
@@ -32,7 +32,7 @@ const char *ScaraKinematics::GetName(bool forStatusReport) const
 
 // Convert Cartesian coordinates to motor coordinates
 // In the following, theta is the proximal arm angle relative to the X axis, psi is the distal arm angle relative to the proximal arm
-bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const
+bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const
 {
 	// No need to limit x,y to reachable positions here, we already did that in class GCodes
 	const float x = machinePos[X_AXIS] + xOffset;
@@ -56,11 +56,11 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa
 	bool switchedMode = false;
 	for (;;)
 	{
-		if (isDefaultArmMode)
+		if (isDefaultArmMode != switchedMode)
 		{
 			// The following equations choose arm mode 0 i.e. distal arm rotated anticlockwise relative to proximal arm
 			theta = atan2f(SCARA_K1 * y - SCARA_K2 * x, SCARA_K1 * x + SCARA_K2 * y);
-			if (theta >= thetaLimits[0])
+			if (theta >= thetaLimits[0] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1])
 			{
 				break;
 			}
@@ -69,7 +69,7 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa
 		{
 			// The following equations choose arm mode 1 i.e. distal arm rotated clockwise relative to proximal arm
 			theta = atan2f(SCARA_K1 * y + SCARA_K2 * x, SCARA_K1 * x - SCARA_K2 * y);
-			if (theta <= thetaLimits[1])
+			if (theta >= thetaLimits[0] && theta <= thetaLimits[1] && psi >= phiLimits[0] && psi <= phiLimits[1])
 			{
 				psi = -psi;
 				break;
@@ -80,10 +80,15 @@ bool ScaraKinematics::CartesianToMotorSteps(const float machinePos[], const floa
 		{
 			return false;		// not reachable
 		}
-		isDefaultArmMode = !isDefaultArmMode;
 		switchedMode = true;
 	}
 
+	// Now that we know we are going to do the move, update the arm mode
+	if (switchedMode)
+	{
+		isDefaultArmMode = !isDefaultArmMode;
+	}
+
 //debugPrintf("psi = %.2f, theta = %.2f\n", psi * RadiansToDegrees, theta * RadiansToDegrees);
 
 	motorPos[X_AXIS] = lrintf(theta * RadiansToDegrees * stepsPerMm[X_AXIS]);
diff --git a/src/Movement/Kinematics/ScaraKinematics.h b/src/Movement/Kinematics/ScaraKinematics.h
index 248d9008..e229468d 100644
--- a/src/Movement/Kinematics/ScaraKinematics.h
+++ b/src/Movement/Kinematics/ScaraKinematics.h
@@ -28,7 +28,7 @@ public:
 	// Overridden base class functions. See Kinematics.h for descriptions.
 	const char *GetName(bool forStatusReport) const override;
 	bool Configure(unsigned int mCode, GCodeBuffer& gb, StringRef& reply, bool& error) override;
-	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[]) const override;
+	bool CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool allowModeChange) const override;
 	void MotorStepsToCartesian(const int32_t motorPos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const override;
 	bool IsReachable(float x, float y) const override;
 	bool LimitPosition(float position[], size_t numAxes, AxesBitmap axesHomed) const override;
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 31890527..f2cc927b 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -569,7 +569,7 @@ void Move::SetPositions(const float move[DRIVES])
 
 void Move::EndPointToMachine(const float coords[], int32_t ep[], size_t numDrives) const
 {
-	if (CartesianToMotorSteps(coords, ep))
+	if (CartesianToMotorSteps(coords, ep, true))
 	{
 		const size_t numAxes = reprap.GetGCodes().GetTotalAxes();
 		for (size_t drive = numAxes; drive < numDrives; ++drive)
@@ -594,9 +594,10 @@ void Move::MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes
 
 // Convert Cartesian coordinates to motor steps, axes only, returning true if successful.
 // Used to perform movement and G92 commands.
-bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes]) const
+bool Move::CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool allowModeChange) const
 {
-	const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(), reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos);
+	const bool b = kinematics->CartesianToMotorSteps(machinePos, reprap.GetPlatform().GetDriveStepsPerUnit(),
+														reprap.GetGCodes().GetVisibleAxes(), reprap.GetGCodes().GetTotalAxes(), motorPos, allowModeChange);
 	if (reprap.Debug(moduleMove) && reprap.Debug(moduleDda))
 	{
 		if (b)
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index 07d00845..9477115e 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -75,7 +75,7 @@ public:
 	// Kinematics and related functions
 	Kinematics& GetKinematics() const { return *kinematics; }
 	bool SetKinematics(KinematicsType k);											// Set kinematics, return true if successful
-	bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes]) const;
+	bool CartesianToMotorSteps(const float machinePos[MaxAxes], int32_t motorPos[MaxAxes], bool allowModeChange) const;
 																					// Convert Cartesian coordinates to delta motor coordinates, return true if successful
 	void MotorStepsToCartesian(const int32_t motorPos[], size_t numVisibleAxes, size_t numTotalAxes, float machinePos[]) const;
 																					// Convert motor coordinates to machine coordinates
diff --git a/src/Version.h b/src/Version.h
index d0ee8670..1cf17174 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -9,7 +9,7 @@
 #define SRC_VERSION_H_
 
 #ifndef VERSION
-# define VERSION "1.20alpha4"
+# define VERSION "1.20alpha5"
 #endif
 
 #ifndef DATE