Fixes to polar kinematics

A and F parameters didn't work because they were not converted from
seconds to step clocks
XY movement was not allowed unless Z was homed
Also added a default implementation of LimitSpeedAndAcceleration in the
Kinematics base class and removed the identical implementations from
RoundBedKinematics, ScaraKinematics and FiveBarScaraKinematics

9 files changed, 35 insertions, 58 deletions

diff --git a/src/Movement/Kinematics/FiveBarScaraKinematics.cpp b/src/Movement/Kinematics/FiveBarScaraKinematics.cpp
index 3e1ff1e1..fe44440b 100644
--- a/src/Movement/Kinematics/FiveBarScaraKinematics.cpp
+++ b/src/Movement/Kinematics/FiveBarScaraKinematics.cpp
@@ -922,22 +922,6 @@ void FiveBarScaraKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd,
 	}
 }
 
-// Limit the speed and acceleration of a move to values that the mechanics can handle. The speeds in Cartesian space have already been limited.
-void FiveBarScaraKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept
-{
-	//TODO should we make use of numVisibleAxes and/or continuousRotationShortcut?
-	// For now we limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration.
-	// Limiting the angular rates of the arms would be better.
-	const float xyFactor = fastSqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS]));
-	if (xyFactor > 0.01)
-	{
-		const Platform& platform = reprap.GetPlatform();
-		const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS));
-		const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS));
-		dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor);
-	}
-}
-
 // Return true if the specified axis is a continuous rotation axis
 bool FiveBarScaraKinematics::IsContinuousRotationAxis(size_t axis) const noexcept
 {
diff --git a/src/Movement/Kinematics/FiveBarScaraKinematics.h b/src/Movement/Kinematics/FiveBarScaraKinematics.h
index 6e7e9b3b..83ba44f7 100644
--- a/src/Movement/Kinematics/FiveBarScaraKinematics.h
+++ b/src/Movement/Kinematics/FiveBarScaraKinematics.h
@@ -40,7 +40,6 @@ public:
 	AxesBitmap GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, const StringRef& filename) const noexcept override;
 	bool QueryTerminateHomingMove(size_t axis) const noexcept override;
 	void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const noexcept override;
-	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept override;
 	bool IsContinuousRotationAxis(size_t axis) const noexcept override;
 	AxesBitmap GetLinearAxes() const noexcept override;
 	AxesBitmap GetConnectedAxes(size_t axis) const noexcept override;
diff --git a/src/Movement/Kinematics/Kinematics.cpp b/src/Movement/Kinematics/Kinematics.cpp
index f6478693..e2ebdc71 100644
--- a/src/Movement/Kinematics/Kinematics.cpp
+++ b/src/Movement/Kinematics/Kinematics.cpp
@@ -207,6 +207,22 @@ bool Kinematics::IsContinuousRotationAxis(size_t axis) const noexcept
 	return false;
 }
 
+// Limit the speed and acceleration of a move to values that the mechanics can handle.
+// The speeds in Cartesian space have already been limited.
+// The default implementation in this class just limits the combined XY speed to the lower of the individual X and Y limits. This is appropriate for
+// many types of kinematics, but not for Cartesian.
+void Kinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept
+{
+	const float xyFactor = fastSqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS]));
+	if (xyFactor > 0.01)
+	{
+		const Platform& platform = reprap.GetPlatform();
+		const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS));
+		const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS));
+		dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor);
+	}
+}
+
 /*static*/ Kinematics *Kinematics::Create(KinematicsType k) noexcept
 {
 	switch (k)
diff --git a/src/Movement/Kinematics/Kinematics.h b/src/Movement/Kinematics/Kinematics.h
index 34fa474a..995c1a76 100644
--- a/src/Movement/Kinematics/Kinematics.h
+++ b/src/Movement/Kinematics/Kinematics.h
@@ -193,7 +193,9 @@ public:
 
 	// Limit the speed and acceleration of a move to values that the mechanics can handle.
 	// The speeds along individual Cartesian axes have already been limited before this is called.
-	virtual void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept = 0;
+	// The default implementation in this class just limits the combined XY speed to the lower of the individual X and Y limits. This is appropriate for
+	// many types of kinematics, but not for Cartesian.
+	virtual void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept;
 
 	// Return true if the specified axis is a continuous rotational axis and G0 commands may choose which direction to move it in
 	virtual bool IsContinuousRotationAxis(size_t axis) const noexcept;
diff --git a/src/Movement/Kinematics/PolarKinematics.cpp b/src/Movement/Kinematics/PolarKinematics.cpp
index 8e9d4af6..565214bb 100644
--- a/src/Movement/Kinematics/PolarKinematics.cpp
+++ b/src/Movement/Kinematics/PolarKinematics.cpp
@@ -39,7 +39,7 @@ DEFINE_GET_OBJECT_MODEL_TABLE(PolarKinematics)
 PolarKinematics::PolarKinematics() noexcept
 	: Kinematics(KinematicsType::polar, SegmentationType(true, false, false)),
 	  minRadius(0.0), maxRadius(DefaultMaxRadius), homedRadius(0.0),
-	  maxTurntableSpeed(DefaultMaxTurntableSpeed), maxTurntableAcceleration(DefaultMaxTurntableAcceleration)
+	  maxTurntableSpeed(ConvertSpeedFromMmPerSec(DefaultMaxTurntableSpeed)), maxTurntableAcceleration(ConvertAcceleration(DefaultMaxTurntableAcceleration))
 {
 	Recalc();
 }
@@ -87,8 +87,16 @@ bool PolarKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const Strin
 		gb.TryGetFValue('H', homedRadius, seen);
 
 		bool seenNonGeometry = TryConfigureSegmentation(gb);
-		gb.TryGetFValue('A', maxTurntableAcceleration, seenNonGeometry);
-		gb.TryGetFValue('F', maxTurntableSpeed, seenNonGeometry);
+		if (gb.Seen('A'))
+		{
+			maxTurntableAcceleration = gb.GetAcceleration();
+			seenNonGeometry = true;
+		}
+		if (gb.Seen('F'))
+		{
+			maxTurntableSpeed = gb.GetSpeedFromMm(true);
+			seenNonGeometry = true;
+		}
 
 		if (seen)
 		{
@@ -97,8 +105,9 @@ bool PolarKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const Strin
 		else if (!seenNonGeometry && !gb.Seen('K'))
 		{
 			Kinematics::Configure(mCode, gb, reply, error);
-			reply.catf(", radius %.1f to %.1fmm, homed radius %.1fmm",
-							(double)minRadius, (double)maxRadius, (double)homedRadius);
+			reply.catf(", radius %.1f to %.1fmm, homed radius %.1fmm, max table acc. %.1fdeg/sec^2, max table speed %.1fdeg/sec",
+							(double)minRadius, (double)maxRadius, (double)homedRadius,
+							(double)InverseConvertAcceleration(maxTurntableAcceleration), (double)InverseConvertSpeedToMmPerSec(maxTurntableSpeed));
 		}
 		return seen;
 	}
@@ -224,9 +233,9 @@ AxesBitmap PolarKinematics::AxesAssumedHomed(AxesBitmap g92Axes) const noexcept
 // Return the set of axes that must be homed prior to regular movement of the specified axes
 AxesBitmap PolarKinematics::MustBeHomedAxes(AxesBitmap axesMoving, bool disallowMovesBeforeHoming) const noexcept
 {
-	if (axesMoving.Intersects(XyzAxes))
+	if (axesMoving.Intersects(XyAxes))
 	{
-		axesMoving |= XyzAxes;
+		axesMoving |= XyAxes;
 	}
 	return axesMoving;
 }
diff --git a/src/Movement/Kinematics/RoundBedKinematics.cpp b/src/Movement/Kinematics/RoundBedKinematics.cpp
index b091e565..7c7dfdf8 100644
--- a/src/Movement/Kinematics/RoundBedKinematics.cpp
+++ b/src/Movement/Kinematics/RoundBedKinematics.cpp
@@ -27,21 +27,6 @@ bool RoundBedKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes)
 	return Kinematics::IsReachable(axesCoords, axes);
 }
 
-// Limit the speed and acceleration of a move to values that the mechanics can handle.
-// The speeds in Cartesian space have already been limited.
-void RoundBedKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept
-{
-	// Limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration
-	const float xyFactor = fastSqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS]));
-	if (xyFactor > 0.01)
-	{
-		const Platform& platform = reprap.GetPlatform();
-		const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS));
-		const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS));
-		dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor);
-	}
-}
-
 // Return a bitmap of axes that move linearly in response to the correct combination of linear motor movements.
 // This is called to determine whether we can babystep the specified axis independently of regular motion.
 AxesBitmap RoundBedKinematics::GetLinearAxes() const noexcept
diff --git a/src/Movement/Kinematics/RoundBedKinematics.h b/src/Movement/Kinematics/RoundBedKinematics.h
index c378a6af..50fc7ae6 100644
--- a/src/Movement/Kinematics/RoundBedKinematics.h
+++ b/src/Movement/Kinematics/RoundBedKinematics.h
@@ -16,7 +16,6 @@ class RoundBedKinematics : public Kinematics
 {
 public:
 	bool IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept override;
-	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept override;
 	AxesBitmap GetLinearAxes() const noexcept override;
 protected:
 	RoundBedKinematics(KinematicsType t, SegmentationType segType) noexcept;
diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp
index 6c9a2e31..9ec01ac9 100644
--- a/src/Movement/Kinematics/ScaraKinematics.cpp
+++ b/src/Movement/Kinematics/ScaraKinematics.cpp
@@ -470,22 +470,6 @@ void ScaraKinematics::OnHomingSwitchTriggered(size_t axis, bool highEnd, const f
 	}
 }
 
-// Limit the speed and acceleration of a move to values that the mechanics can handle.
-// The speeds in Cartesian space have already been limited.
-void ScaraKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept
-{
-	// For now we limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration.
-	// Limiting the angular rates of the arms would be better.
-	const float xyFactor = fastSqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS]));
-	if (xyFactor > 0.01)
-	{
-		const Platform& platform = reprap.GetPlatform();
-		const float maxSpeed = min<float>(platform.MaxFeedrate(X_AXIS), platform.MaxFeedrate(Y_AXIS));
-		const float maxAcceleration = min<float>(platform.Acceleration(X_AXIS), platform.Acceleration(Y_AXIS));
-		dda.LimitSpeedAndAcceleration(maxSpeed/xyFactor, maxAcceleration/xyFactor);
-	}
-}
-
 // Return true if the specified axis is a continuous rotation axis
 bool ScaraKinematics::IsContinuousRotationAxis(size_t axis) const noexcept
 {
diff --git a/src/Movement/Kinematics/ScaraKinematics.h b/src/Movement/Kinematics/ScaraKinematics.h
index 003e483f..6dee145d 100644
--- a/src/Movement/Kinematics/ScaraKinematics.h
+++ b/src/Movement/Kinematics/ScaraKinematics.h
@@ -41,7 +41,6 @@ public:
 	AxesBitmap GetHomingFileName(AxesBitmap toBeHomed, AxesBitmap alreadyHomed, size_t numVisibleAxes, const StringRef& filename) const noexcept override;
 	bool QueryTerminateHomingMove(size_t axis) const noexcept override;
 	void OnHomingSwitchTriggered(size_t axis, bool highEnd, const float stepsPerMm[], DDA& dda) const noexcept override;
-	void LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept override;
 	bool IsContinuousRotationAxis(size_t axis) const noexcept override;
 	AxesBitmap GetLinearAxes() const noexcept override;