Initial implementation of G68/69

4 files changed, 128 insertions, 22 deletions

diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 1c2c5427..a805f5c0 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -239,6 +239,10 @@ void GCodes::Reset() noexcept
 		}
 	}
 
+#if SUPPORT_COORDINATE_ROTATION
+	g68Angle = g68Centre[0] = g68Centre[1] = 0.0;			// no coordinate rotation
+#endif
+
 	moveState.currentCoordinateSystem = 0;
 
 	for (float& f : moveState.coords)
@@ -1970,6 +1974,13 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated, const char *& e
 		}
 	}
 
+#if SUPPORT_COORDINATE_ROTATION
+	if (moveState.moveType == 0 && rp == nullptr && !gb.LatestMachineState().g53Active && !gb.LatestMachineState().runningSystemMacro)
+	{
+		RotateCoordinates(g68Angle, moveState.currentUserPosition);
+	}
+#endif
+
 	// Check enough axes have been homed
 	switch (moveState.moveType)
 	{
@@ -2180,47 +2191,47 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 	}
 
 	// Get the axis parameters
-	float newAxis0Pos, newAxis1Pos;
+	float newAxisPos[2];
 	if (gb.Seen(axisLetters[axis0]))
 	{
-		newAxis0Pos = gb.GetDistance();
+		newAxisPos[0] = gb.GetDistance();
 		if (gb.LatestMachineState().axesRelative)
 		{
-			newAxis0Pos += moveState.initialUserC0;
+			newAxisPos[0] += moveState.initialUserC0;
 		}
 		else if (gb.LatestMachineState().g53Active)
 		{
-			newAxis0Pos += GetCurrentToolOffset(axis0);
+			newAxisPos[0] += GetCurrentToolOffset(axis0);
 		}
 		else if (!gb.LatestMachineState().runningSystemMacro)
 		{
-			newAxis0Pos += GetWorkplaceOffset(axis0);
+			newAxisPos[0] += GetWorkplaceOffset(axis0);
 		}
 	}
 	else
 	{
-		newAxis0Pos = moveState.initialUserC0;
+		newAxisPos[0] = moveState.initialUserC0;
 	}
 
 	if (gb.Seen(axisLetters[axis1]))
 	{
-		newAxis1Pos = gb.GetDistance();
+		newAxisPos[1] = gb.GetDistance();
 		if (gb.LatestMachineState().axesRelative)
 		{
-			newAxis1Pos += moveState.initialUserC1;
+			newAxisPos[1] += moveState.initialUserC1;
 		}
 		else if (gb.LatestMachineState().g53Active)
 		{
-			newAxis1Pos += GetCurrentToolOffset(axis1);
+			newAxisPos[1] += GetCurrentToolOffset(axis1);
 		}
 		else if (!gb.LatestMachineState().runningSystemMacro)
 		{
-			newAxis1Pos += GetWorkplaceOffset(axis1);
+			newAxisPos[1] += GetWorkplaceOffset(axis1);
 		}
 	}
 	else
 	{
-		newAxis1Pos = moveState.initialUserC1;
+		newAxisPos[1] = moveState.initialUserC1;
 	}
 
 	float iParam, jParam;
@@ -2230,8 +2241,8 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 		const float rParam = gb.GetDistance();
 
 		// Get the XY coordinates of the midpoints between the start and end points X and Y distances between start and end points
-		const float deltaAxis0 = newAxis0Pos - moveState.initialUserC0;
-		const float deltaAxis1 = newAxis1Pos - moveState.initialUserC1;
+		const float deltaAxis0 = newAxisPos[0] - moveState.initialUserC0;
+		const float deltaAxis1 = newAxisPos[1] - moveState.initialUserC1;
 
 		const float dSquared = fsquare(deltaAxis0) + fsquare(deltaAxis1);	// square of the distance between start and end points
 
@@ -2301,12 +2312,20 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 	memcpyf(moveState.initialCoords, moveState.coords, numVisibleAxes);
 
 	// Save the arc centre user coordinates for later
-	const float userArcCentreAxis0 = moveState.initialUserC0 + iParam;
-	const float userArcCentreAxis1 = moveState.initialUserC1 + jParam;
+	float userArcCentre[2] = { moveState.initialUserC0 + iParam, moveState.initialUserC1 + jParam };
 
-	// Work out the new user position
-	moveState.currentUserPosition[axis0] = newAxis0Pos;
-	moveState.currentUserPosition[axis1] = newAxis1Pos;
+#if SUPPORT_COORDINATE_ROTATION
+	// Apply coordinate rotation to the final and the centre coordinates
+	if (!gb.LatestMachineState().g53Active && !gb.LatestMachineState().runningSystemMacro && gb.LatestMachineState().selectedPlane == 0)
+	{
+		RotateCoordinates(g68Angle, newAxisPos);
+		RotateCoordinates(g68Angle, userArcCentre);
+	}
+#endif
+
+	// Set the new user position
+	moveState.currentUserPosition[axis0] = newAxisPos[0];
+	moveState.currentUserPosition[axis1] = newAxisPos[1];
 
 	// CNC machines usually do a full circle if the initial and final XY coordinates are the same.
 	// Usually this is because X and Y were not given, but repeating the coordinates is permitted.
@@ -2357,7 +2376,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 	}
 
 	// Compute the angle at which we stop
-	const float finalTheta = atan2(moveState.currentUserPosition[axis1] - userArcCentreAxis1, moveState.currentUserPosition[axis0] - userArcCentreAxis0);
+	const float finalTheta = atan2(moveState.currentUserPosition[axis1] - userArcCentre[1], moveState.currentUserPosition[axis0] - userArcCentre[0]);
 
 	// Set up default move parameters
 	moveState.checkEndstops = false;
@@ -2375,11 +2394,11 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err)
 	{
 		if (axis0Mapping.IsBitSet(axis))
 		{
-			moveState.arcCentre[axis] = (userArcCentreAxis0 * axisScaleFactors[axis]) + currentBabyStepOffsets[axis] - Tool::GetOffset(reprap.GetCurrentTool(), axis);
+			moveState.arcCentre[axis] = (userArcCentre[0] * axisScaleFactors[axis]) + currentBabyStepOffsets[axis] - Tool::GetOffset(reprap.GetCurrentTool(), axis);
 		}
 		else if (axis1Mapping.IsBitSet(axis))
 		{
-			moveState.arcCentre[axis] = (userArcCentreAxis1 * axisScaleFactors[axis]) + currentBabyStepOffsets[axis] - Tool::GetOffset(reprap.GetCurrentTool(), axis);
+			moveState.arcCentre[axis] = (userArcCentre[1] * axisScaleFactors[axis]) + currentBabyStepOffsets[axis] - Tool::GetOffset(reprap.GetCurrentTool(), axis);
 		}
 	}
 
@@ -4399,9 +4418,17 @@ float GCodes::GetCurrentToolOffset(size_t axis) const noexcept
 	return (tool == nullptr) ? 0.0 : tool->GetOffset(axis);
 }
 
-// Get the current user coordinate and remove the workplace offset
+// Get the current user coordinate and remove the coordinate rotation and workplace offset
 float GCodes::GetUserCoordinate(size_t axis) const noexcept
 {
+#if SUPPORT_COORDINATE_ROTATION
+	if (g68Angle != 0.0 && axis < 2)
+	{
+		float temp[2] = { moveState.currentUserPosition[0], moveState.currentUserPosition[1] };
+		RotateCoordinates(-g68Angle, temp);
+		return temp[axis] - GetWorkplaceOffset(axis);
+	}
+#endif
 	return (axis < numTotalAxes) ? moveState.currentUserPosition[axis] - GetWorkplaceOffset(axis) : 0.0;
 }
 
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index 261a069d..add5506e 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -242,6 +242,10 @@ public:
 
 	unsigned int GetWorkplaceCoordinateSystemNumber() const noexcept { return moveState.currentCoordinateSystem + 1; }
 
+#if SUPPORT_COORDINATE_ROTATION
+	void RotateCoordinates(float angleDegrees, float coords[2]) const noexcept;		// Account for coordinate rotation
+#endif
+
 	// This function is called by other functions to account correctly for workplace coordinates
 	float GetWorkplaceOffset(size_t axis) const noexcept
 	{
@@ -254,6 +258,11 @@ public:
 		return workplaceCoordinates[workplaceNumber][axis];
 	}
 
+# if SUPPORT_COORDINATE_ROTATION
+	float GetRotationAngle() const noexcept { return g68Angle; }
+	float GetRotationCentre(size_t index) const noexcept pre(index < 2) { return g68Centre[index]; }
+# endif
+
 	size_t GetNumInputs() const noexcept { return NumGCodeChannels; }
 	const GCodeBuffer* GetInput(size_t n) const noexcept { return gcodeSources[n]; }
 	const GCodeBuffer* GetInput(GCodeChannel n) const noexcept { return gcodeSources[n.RawValue()]; }
@@ -509,6 +518,10 @@ private:
 	void SetMoveBufferDefaults() noexcept;										// Set up default values in the move buffer
 	void ChangeExtrusionFactor(unsigned int extruder, float factor) noexcept;	// Change a live extrusion factor
 
+#if SUPPORT_COORDINATE_ROTATION
+	GCodeResult HandleG68(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);	// Handle G68
+#endif
+
 #if SUPPORT_12864_LCD
 	int GetHeaterNumber(unsigned int itemNumber) const noexcept;
 #endif
@@ -604,6 +617,11 @@ private:
 
 	float workplaceCoordinates[NumCoordinateSystems][MaxAxes];	// Workplace coordinate offsets
 
+#if SUPPORT_COORDINATE_ROTATION
+	float g68Angle;								// the G68 rotation angle in radians
+	float g68Centre[2];							// the XY coordinates of the centre to rotate about
+#endif
+
 #if HAS_MASS_STORAGE || HAS_EMBEDDED_FILES
 	FileData fileToPrint;						// The next file to print
 #endif
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index 0017115e..cd1ff11e 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -425,6 +425,16 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			result = SavePosition(gb, reply);
 			break;
 
+#if SUPPORT_COORDINATE_ROTATION
+		case 68:
+			result = HandleG68(gb, reply);
+			break;
+
+		case 69:
+			g68Angle = 0.0;
+			break;
+#endif
+
 		case 90: // Absolute coordinates
 			gb.LatestMachineState().axesRelative = false;
 			reprap.InputsUpdated();
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index a48d870e..333fbc90 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -1632,6 +1632,57 @@ GCodeResult GCodes::ConfigureLocalDriverBasicParameters(GCodeBuffer& gb, const S
 	return GCodeResult::ok;
 }
 
+#if SUPPORT_COORDINATE_ROTATION
+
+// Handle G68
+GCodeResult GCodes::HandleG68(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException)
+{
+	if (!LockMovementAndWaitForStandstill(gb))
+	{
+		return GCodeResult::notFinished;
+	}
+	if (gb.CurrentFileMachineState().selectedPlane != 0)
+	{
+		reply.copy("this command may only be used when the selected plane is XY");
+		return GCodeResult::error;
+	}
+
+	float angle, centreX, centreY;
+	gb.MustSee('R');
+	angle = gb.GetFValue();
+	gb.MustSee('A');
+	centreX = gb.GetFValue();
+	gb.MustSee('B');
+	centreY= gb.GetFValue();
+
+	g68Centre[0] = centreX + GetWorkplaceOffset(0);
+	g68Centre[1] = centreY + GetWorkplaceOffset(1);
+	if (gb.Seen('I'))
+	{
+		g68Angle += angle;
+	}
+	else
+	{
+		g68Angle = angle;
+	}
+	return GCodeResult::ok;
+}
+
+// Account for coordinate rotation
+void GCodes::RotateCoordinates(float angleDegrees, float coords[2]) const noexcept
+{
+	if (angleDegrees != 0.0)
+	{
+		const float angle = angleDegrees * DegreesToRadians;
+		const float newX = (coords[0] - g68Centre[0]) * cosf(angle)    + (coords[1] - g68Centre[1]) * sinf(angle) + g68Centre[0];
+		const float newY = (coords[0] - g68Centre[0]) * (-sinf(angle)) + (coords[1] - g68Centre[1]) * cosf(angle) + g68Centre[1];
+		coords[0] = newX;
+		coords[1] = newY;
+	}
+}
+
+#endif
+
 // Change a live extrusion factor
 void GCodes::ChangeExtrusionFactor(unsigned int extruder, float factor) noexcept
 {